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);
}
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); }
}
}
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();
}
}
/*
* 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);
}
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;
}
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;
}
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);
}
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); }
}
}
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);
}
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;
}
}
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();
}
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;
}
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);
}
}
}
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;
}
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;
}
}
