/**************************************************************************** * * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file navigator_mission.cpp * * Helper class to access missions * * @author Julian Oes */ #include #include #include #include #include #include #include #include #include #include #include #include "navigator.h" #include "mission.h" Mission::Mission(Navigator *navigator, const char *name) : SuperBlock(NULL, name), _navigator(navigator), _first_run(true), _param_onboard_enabled(this, "ONBOARD_EN"), _param_loiter_radius(this, "LOITER_RAD"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _mission_item({0}), _mission_result_pub(-1), _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _loiter_set(false) { /* load initial params */ updateParams(); /* set initial mission items */ reset(); } Mission::~Mission() { } void Mission::reset() { _first_run = true; _loiter_set = false; } bool Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { /* check if anything has changed */ bool onboard_updated = is_onboard_mission_updated(); bool offboard_updated = is_offboard_mission_updated(); bool updated = false; /* reset mission items if needed */ if (onboard_updated || offboard_updated) { set_mission_items(pos_sp_triplet); updated = true; _first_run = false; } /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { advance_mission(); set_mission_items(pos_sp_triplet); updated = true; } /* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */ if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) { bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached); set_loiter_item(use_current_pos_sp, pos_sp_triplet); updated = true; _loiter_set = true; } return updated; } bool Mission::is_mission_item_reached() { /* TODO: count turns */ #if 0 if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { return false; } #endif hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* require only altitude for takeoff */ if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { _waypoint_position_reached = true; } } else { if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { _waypoint_position_reached = true; } } } if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; } } else { _waypoint_yaw_reached = true; } } /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; // if (_mission_item.time_inside > 0.01f) { // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", // (double)_mission_item.time_inside); // } } /* check if the MAV was long enough inside the waypoint orbit */ if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { return true; } } return false; } void Mission::reset_mission_item_reached() { _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_first_inside_orbit = 0; } void Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; } else if (item->nav_cmd == NAV_CMD_LAND) { sp->type = SETPOINT_TYPE_LAND; } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { sp->type = SETPOINT_TYPE_LOITER; } else { sp->type = SETPOINT_TYPE_POSITION; } } void Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet) { if (reuse_current_pos_sp && pos_sp_triplet->current.valid) { /* nothing to be done, just use the current item */ } else { pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ } pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get(); pos_sp_triplet->current.loiter_direction = 1; pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = true; pos_sp_triplet->next.valid = false; } bool Mission::is_onboard_mission_updated() { bool updated; orb_check(_navigator->get_onboard_mission_sub(), &updated); if (!updated && !_first_run) { return false; } if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ if (_onboard_mission.current_index >=0 && _onboard_mission.current_index < (int)_onboard_mission.count) { _current_onboard_mission_index = _onboard_mission.current_index; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { _current_onboard_mission_index = 0; /* if not initialized, set it to 0 */ } else if (_current_onboard_mission_index < 0) { _current_onboard_mission_index = 0; } /* otherwise, just leave it */ } } else { _onboard_mission.count = 0; _onboard_mission.current_index = 0; _current_onboard_mission_index = 0; } return true; } bool Mission::is_offboard_mission_updated() { bool updated; orb_check(_navigator->get_offboard_mission_sub(), &updated); if (!updated && !_first_run) { return false; } if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { /* determine current index */ if (_offboard_mission.current_index >= 0 && _offboard_mission.current_index < (int)_offboard_mission.count) { _current_offboard_mission_index = _offboard_mission.current_index; } else { /* if less WPs available, reset to first WP */ 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; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t)_offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { _offboard_mission.count = 0; _offboard_mission.current_index = 0; _current_offboard_mission_index = 0; } return true; } void Mission::advance_mission() { switch (_mission_type) { case MISSION_TYPE_ONBOARD: _current_onboard_mission_index++; break; case MISSION_TYPE_OFFBOARD: _current_offboard_mission_index++; break; case MISSION_TYPE_NONE: default: break; } } void Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting onboard mission item */ _mission_type = MISSION_TYPE_ONBOARD; _loiter_set = false; } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { /* try setting offboard mission item */ _mission_type = MISSION_TYPE_OFFBOARD; _loiter_set = false; } else { _mission_type = MISSION_TYPE_NONE; } } void Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, struct position_setpoint_s *previous_pos_sp) { /* reuse current setpoint as previous setpoint */ if (current_pos_sp->valid) { memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); } } bool Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { if (_param_onboard_enabled.get() > 0 && _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; reset_mission_item_reached(); /* TODO: report this somehow */ memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } } return false; } bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { if (_current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } struct mission_item_s new_mission_item; if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { /* convert the current mission item and set it valid */ mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); current_pos_sp->valid = true; reset_mission_item_reached(); report_current_offboard_mission_item(); memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; } else { warnx("ERROR: WP read fail"); } } return false; } void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { int next_temp_mission_index = _onboard_mission.current_index + 1; /* try if there is a next onboard mission */ if (next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); next_pos_sp->valid = true; return; } } /* give up */ next_pos_sp->valid = false; return; } void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ int next_temp_mission_index = _offboard_mission.current_index + 1; if (next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } struct mission_item_s new_mission_item; if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); next_pos_sp->valid = true; return; } } /* give up */ next_pos_sp->valid = false; return; } bool Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i=0; i<10; i++) { const ssize_t len = sizeof(struct mission_item_s); /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { return false; } /* only raise the repeat count if this is for the current mission item * but not for the next mission item */ if (is_current) { (new_mission_item->do_jump_current_count)++; /* save repeat count */ if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ return false; } /* TODO: report about DO JUMP 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 = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ return true; } } /* we have given up, we don't want to cycle forever */ warnx("ERROR: cycling through mission items without success"); return false; } void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; _mission_result.mission_index_reached = _current_offboard_mission_index; } publish_mission_result(); } void Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); } void Mission::publish_mission_result() { /* lazily publish the mission result only once available */ if (_mission_result_pub > 0) { /* publish mission result */ orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); } else { /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ _mission_result.mission_reached = false; }