Exemplo n.º 1
0
void Search::SetExample() {
	// Test changing the missions waypoint count?
	search_mission.count = 2;
	search_mission.current_seq = 0;
	getCoord(15.0);
	dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 0, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s));
	offboard = (offboard == 0) ? 1 : 0;
	getCoord(15.0, 90);
	next_point.nav_cmd = NAV_CMD_WAYPOINT;
	dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 1, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s));
	offboard = (offboard == 0) ? 1 : 0;
	orb_publish(ORB_ID(offboard_mission), mission_pub, &search_mission);
}
Exemplo n.º 2
0
void
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
{
	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
	struct mission_item_s mission_item;

	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
		_time_last_sent = hrt_absolute_time();

		/* create mission_item_s from mavlink_mission_item_t */
		mavlink_mission_item_t wp;
		format_mavlink_mission_item(&mission_item, &wp);

		wp.target_system = sysid;
		wp.target_component = compid;
		wp.seq = seq;
		wp.current = (_current_seq == seq) ? 1 : 0;

		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);

		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }

	} else {
		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
		_mavlink->send_statustext_critical("Unable to read from micro SD");

		if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
	}
}
Exemplo n.º 3
0
void
Mission::check_mission_valid(bool force)
{

	if ((!_home_inited && _navigator->home_position_valid()) || force) {

		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);

		_navigator->get_mission_result()->valid =
			_missionFeasibilityChecker.checkMissionFeasible(
				_navigator->get_mavlink_log_pub(),
				(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
				_navigator->get_home_position()->alt,
				_navigator->home_position_valid(),
				_navigator->get_global_position()->lat,
				_navigator->get_global_position()->lon,
				_param_dist_1wp.get(),
				_navigator->get_mission_result()->warning,
				_navigator->get_default_acceptance_radius(),
				_navigator->get_land_detected()->landed);

		_navigator->increment_mission_instance_count();
		_navigator->set_mission_result_updated();
		_home_inited = _navigator->home_position_valid();
	}
}
Exemplo n.º 4
0
/*
 * Possible Bug: We assume Home Position is already set by commander.
 */
Search::Search() {
	// Clear out stuct data
	memset(&search_mission, 0, sizeof (struct mission_s));
	memset(&next_point, 0, sizeof (struct mission_item_s));
	memset(&last, 0, sizeof (struct home_position_s));
	memset(&home, 0, sizeof (struct home_position_s));
	// Get Current Mission!
	int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	orb_copy(ORB_ID(offboard_mission), mission_sub, &search_mission);
	orb_unsubscribe(mission_sub);
	// Set Start Position to Home Position
	int home_sub = orb_subscribe(ORB_ID(home_position));
	orb_copy(ORB_ID(home_position), home_sub, &home);
	orb_unsubscribe(home_sub);
	// Read in the last waypoint on the offboard mission, store it to a local variable
	dm_read(DM_KEY_WAYPOINTS_OFFBOARD(offboard), 0, &last, sizeof (struct mission_item_s));
	offboard = 1;
	// Initialize Next Waypoint
	next_point.altitude_is_relative = true;
	next_point.altitude = home.alt + 7;
	next_point.autocontinue = true;
	next_point.nav_cmd = NAV_CMD_TAKEOFF;
	next_point.acceptance_radius = 5;
	next_point.lat = home.lat;
	next_point.lon = home.lon;
	next_point.time_inside = 0;
	next_point.frame = last.frame;
	mission_pub = orb_advertise(ORB_ID(offboard_mission), &search_mission);
}
Exemplo n.º 5
0
unsigned
Mission::find_offboard_land_start()
{
	/* find the first MAV_CMD_DO_LAND_START and return the index
	 *  return -1 if not found
	 *
	 * TODO: implement full spec and find closest landing point geographically
	 */

	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);

	for (size_t i = 0; i < _offboard_mission.count; i++) {
		struct mission_item_s missionitem;
		const ssize_t len = sizeof(missionitem);
		if (dm_read(dm_current, i, &missionitem, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			return -1;
		}

		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
			return i;
		}
	}

	return -1;
}
Exemplo n.º 6
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;

			/* if not initialized, set it to 0 */
			} else if (_current_offboard_mission_index < 0) {
				_current_offboard_mission_index = 0;
			}
			/* otherwise, just leave it */
		}

		/* Check mission feasibility, for now do not handle the return value,
		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);

		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
				_navigator->get_land_detected()->landed);

		_navigator->get_mission_result()->valid = !failed;
		if (!failed) {
			/* reset mission failure if we have an updated valid mission */
			_navigator->get_mission_result()->mission_failure = false;
		}
		_navigator->increment_mission_instance_count();
		_navigator->set_mission_result_updated();

	} 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();
}
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
		bool land_start_req)
{
	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
	const size_t nMissionItems = mission.count;

	const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);

	Geofence &geofence = _navigator->get_geofence();
	fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
	const float home_alt = _navigator->get_home_position()->alt;
	const bool home_valid = _navigator->home_position_valid();
	const bool home_alt_valid = _navigator->home_alt_valid();

	bool &warning_issued = _navigator->get_mission_result()->warning;
	const float default_acceptance_rad  = _navigator->get_default_acceptance_radius();
	const float default_altitude_acceptance_rad  = _navigator->get_altitude_acceptance_radius();
	const bool landed = _navigator->get_land_detected()->landed;

	bool failed = false;
	bool warned = false;

	// first check if we have a valid position
	if (!home_alt_valid) {
		failed = true;
		warned = true;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");

	} else {
		failed = failed ||
			 !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
	}

	// check if all mission item commands are supported
	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
	failed = failed
		 || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);

	if (isRotarywing) {
		failed = failed
			 || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);

	} else {
		failed = failed
			 || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
					    default_acceptance_rad, land_start_req);
	}

	return !failed;
}
Exemplo n.º 8
0
void Search::SetMission(double search_radius) {
	// Get the amount of waypoints needed for mission.
	// Note: mission should already have a size of 1. For endpoint.
	// Note: we won't need to manage the home waypoint ourselves.
	// Note: We assume  the last waypoint is the upper left corner of the square search pattern.
	// Note: Only test on the field where the home position is correctly set
	// Home position is not part of mission, Index 0 = First new waypoint

	/* Waypoint ID 
	 * Waypoint Count = Waypoint ID + 1*/
	dm_clear(DM_KEY_WAYPOINTS_OFFBOARD(offboard));
	offboard = 0;
	int wp_id = 0;
	// Note: Acceptance Radius not accounted for (it's in meters, not degrees))
	while (next_point.lat - last.lat > search_radius && next_point.lon - last.lon > search_radius) {
		if (wp_id % 2 == 0) {
			// Do Latitudinal shifts here
			if (next_point.lat - last.lat < search_radius) {
				// Going Up
				next_point.lat = last.lat;
			} else {
				// Going down
				next_point.lat = home.lat;
			}
		} else {
			// Do longitudinal shifts here
			next_point.lon += -1 * search_radius;
		}
		if (wp_id)
			next_point.nav_cmd = NAV_CMD_WAYPOINT;
		dm_write(DM_KEY_WAYPOINTS_OFFBOARD(offboard), wp_id++, DM_PERSIST_POWER_ON_RESET, &next_point, sizeof(struct mission_item_s));
		offboard = (offboard == 0) ? 1 : 0;
	}
	search_mission.count = wp_id + 1;
	warnx("Waypoint count is: %d", search_mission.count);
	search_mission.current_seq = 0;
	orb_publish(ORB_ID(offboard_mission), mission_pub, &search_mission);
}
Exemplo n.º 9
0
void
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
{
	dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
	struct mission_item_s mission_item;

	if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
		_time_last_sent = hrt_absolute_time();

		if (_int_mode) {
			mavlink_mission_item_int_t wp;
			format_mavlink_mission_item(&mission_item, reinterpret_cast<mavlink_mission_item_t *>(&wp));

			wp.target_system = sysid;
			wp.target_component = compid;
			wp.seq = seq;
			wp.current = (_current_seq == seq) ? 1 : 0;

			mavlink_msg_mission_item_int_send_struct(_mavlink->get_channel(), &wp);

			if (_verbose) {
				PX4_INFO("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
			}

		} else {
			mavlink_mission_item_t wp;
			format_mavlink_mission_item(&mission_item, &wp);

			wp.target_system = sysid;
			wp.target_component = compid;
			wp.seq = seq;
			wp.current = (_current_seq == seq) ? 1 : 0;

			mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp);

			if (_verbose) {
				PX4_INFO("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system);
			}
		}

	} else {
		send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);

		if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
			_mavlink->send_statustext_critical("Mission storage: Unable to read from microSD");
		}

		if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
	}
}
Exemplo n.º 10
0
void
Mission::reset_offboard_mission(struct mission_s &mission)
{
	dm_lock(DM_KEY_MISSION_STATE);

	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
			/* set current item to 0 */
			mission.current_seq = 0;

			/* reset jump counters */
			if (mission.count > 0) {
				dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);

				for (unsigned index = 0; index < mission.count; index++) {
					struct mission_item_s item;
					const ssize_t len = sizeof(struct mission_item_s);

					if (dm_read(dm_current, index, &item, len) != len) {
						PX4_WARN("could not read mission item during reset");
						break;
					}

					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
						item.do_jump_current_count = 0;

						if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET,
							     &item, len) != len) {
							PX4_WARN("could not save mission item during reset");
							break;
						}
					}
				}
			}

		} else {
			mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: could not read mission");

			/* initialize mission state in dataman */
			mission.dataman_id = 0;
			mission.count = 0;
			mission.current_seq = 0;
		}

		dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
	}

	dm_unlock(DM_KEY_MISSION_STATE);
}
Exemplo n.º 11
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();
		}

		/* 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->get_global_position()->lat, _navigator->get_global_position()->lon,
					_param_dist_1wp.get(), _navigator->get_mission_result()->warning);

			_navigator->increment_mission_instance_count();
			_navigator->set_mission_result_updated();

			_home_inited = true;
		}

	} else {
		/* read mission topics on initialization */
		_inited = true;

		update_onboard_mission();
		update_offboard_mission();
	}

	/* require takeoff after non-loiter or landing */
	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
		_need_takeoff = true;
	}
}
Exemplo n.º 12
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;

                /* if not initialized, set it to 0 */
            } else if (_current_offboard_mission_index < 0) {
                _current_offboard_mission_index = 0;
            }
            /* otherwise, just leave it */
        }

        /* Check mission feasibility, for now do not handle the return value,
         * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
        dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);

        failed = !_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());

    } else {
        warnx("offboard mission update failed");
    }

    if (failed) {
        _offboard_mission.count = 0;
        _offboard_mission.current_seq = 0;
        _current_offboard_mission_index = 0;
    }

    set_current_offboard_mission_item();
}
Exemplo n.º 13
0
bool
Mission::check_mission_valid()
{
	/* check if the home position became valid in the meantime */
	if (!_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->get_global_position()->lat, _navigator->get_global_position()->lon,
				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius());

		_navigator->increment_mission_instance_count();
		_navigator->set_mission_result_updated();

		_home_inited = true;
	}

	return _navigator->get_mission_result()->valid;
}
Exemplo n.º 14
0
void
MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
{
	mavlink_mission_item_t wp;
	mavlink_msg_mission_item_decode(msg, &wp);

	if (CHECK_SYSID_COMPID_MISSION(wp)) {
		if (_state == MAVLINK_WPM_STATE_GETLIST) {
			_time_last_recv = hrt_absolute_time();

			if (wp.seq != _transfer_seq) {
				if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); }

				/* don't send request here, it will be performed in eventloop after timeout */
				return;
			}

		} else if (_state == MAVLINK_WPM_STATE_IDLE) {
			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); }

			_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
			return;

		} else {
			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); }

			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
			return;
		}

		struct mission_item_s mission_item;
		int ret = parse_mavlink_mission_item(&wp, &mission_item);

		if (ret != OK) {
			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }

			_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");

			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
			_state = MAVLINK_WPM_STATE_IDLE;
			_transfer_in_progress = false;
			return;
		}

		dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);

		if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
			if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }

			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
			_mavlink->send_statustext_critical("Unable to write on micro SD");
			_state = MAVLINK_WPM_STATE_IDLE;
			_transfer_in_progress = false;
			return;
		}

		/* waypoint marked as current */
		if (wp.current) {
			_transfer_current_seq = wp.seq;
		}

		if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }

		_transfer_seq = wp.seq + 1;

		if (_transfer_seq == _transfer_count) {
			/* got all new mission items successfully */
			if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }

			_mavlink->send_statustext_info("WPM: Transfer complete.");

			_state = MAVLINK_WPM_STATE_IDLE;

			if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);

			} else {
				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
			}
			_transfer_in_progress = false;

		} else {
			/* request next item */
			send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
		}
	}
}
Exemplo n.º 15
0
bool
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{
	/* select onboard/offboard mission */
	int *mission_index_ptr;
	dm_item_t dm_item;

	struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
	int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;

	/* do not work on empty missions */
	if (mission->count == 0) {
		return false;
	}

	/* move to next item if there is one */
	if (mission_index_next < ((int)mission->count - 1)) {
		mission_index_next++;
	}

	if (onboard) {
		/* onboard mission */
		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
		dm_item = DM_KEY_WAYPOINTS_ONBOARD;

	} else {
		/* offboard mission */
		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
	}

	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
	for (int i = 0; i < 10; i++) {

		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
			/* mission item index out of bounds - if they are equal, we just reached the end */
			if (*mission_index_ptr != (int)mission->count) {
				mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
			}
			return false;
		}

		const ssize_t len = sizeof(struct mission_item_s);

		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
		struct mission_item_s mission_item_tmp;

		/* read mission item from datamanager */
		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
			                     "ERROR waypoint could not be read");
			return false;
		}

		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {

			/* do DO_JUMP as many times as requested */
			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {

				/* only raise the repeat count if this is for the current mission item
				* but not for the next mission item */
				if (is_current) {
					(mission_item_tmp.do_jump_current_count)++;
					/* save repeat count */
					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
					    &mission_item_tmp, len) != len) {
						/* not supposed to happen unless the datamanager can't access the
						 * dataman */
						mavlink_log_critical(_navigator->get_mavlink_fd(),
								     "ERROR DO JUMP waypoint could not be written");
						return false;
					}
					report_do_jump_mission_changed(*mission_index_ptr,
								       mission_item_tmp.do_jump_repeat_count);
				}
				/* set new mission item index and repeat
				* we don't have to validate here, if it's invalid, we should realize this later .*/
				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;

			} else {
				if (is_current) {
					mavlink_log_critical(_navigator->get_mavlink_fd(),
							     "DO JUMP repetitions completed");
				}
				/* no more DO_JUMPS, therefore just try to continue with next mission item */
				(*mission_index_ptr)++;
			}

		} else {
			/* if it's not a DO_JUMP, then we were successful */
			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
			return true;
		}
	}

	/* we have given up, we don't want to cycle forever */
	mavlink_log_critical(_navigator->get_mavlink_fd(),
			     "ERROR DO JUMP is cycling, giving up");
	return false;
}
Exemplo n.º 16
0
bool
Mission::check_dist_1wp()
{
    if (_dist_1wp_ok) {
        /* always return true after at least one successful check */
        return true;
    }

    /* check if first waypoint is not too far from home */
    if (_param_dist_1wp.get() > 0.0f) {
        if (_navigator->get_vstatus()->condition_home_position_valid) {
            struct mission_item_s mission_item;

            /* find first waypoint (with lat/lon) item in datamanager */
            for (unsigned i = 0; i < _offboard_mission.count; i++) {
                if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
                            &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {

                    /* check only items with valid lat/lon */
                    if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
                            mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
                            mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
                            mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
                            mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
                            mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {

                        /* check distance from current position to item */
                        float dist_to_1wp = get_distance_to_next_waypoint(
                                                mission_item.lat, mission_item.lon,
                                                _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

                        if (dist_to_1wp < _param_dist_1wp.get()) {
                            _dist_1wp_ok = true;
                            if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
                                /* allow at 2/3 distance, but warn */
                                mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
                            }
                            return true;

                        } else {
                            /* item is too far from home */
                            mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
                            return false;
                        }
                    }

                } else {
                    /* error reading, mission is invalid */
                    mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
                    return false;
                }
            }

            /* no waypoints found in mission, then we will not fly far away */
            _dist_1wp_ok = true;
            return true;

        } else {
            mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
            return false;
        }

    } else {
        return true;
    }
}