From 8e8798a5225d333596c9018ec703da0c6787493d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Jun 2014 23:39:17 +0200 Subject: [PATCH] navigator: spaces/tabs fixed, old commented code removed --- src/modules/navigator/navigator_main.cpp | 322 ----------------------- src/modules/navigator/rtl.cpp | 72 ++--- 2 files changed, 36 insertions(+), 358 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 493e86d4a7..e22c4d72df 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -446,329 +446,7 @@ Navigator::status() warnx("Geofence not set"); } } -#if 0 -bool -Navigator::start_none_on_ground() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_none_in_air() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_auto_on_ground() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - _update_triplet = true; - return true; -} - -bool -Navigator::start_loiter() -{ - /* if no existing item available, use current position */ - if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _pos_sp_triplet.current.alt = _global_pos.alt; - } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _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; - - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - - _update_triplet = true; - return true; -} - -bool -Navigator::start_mission() -{ - /* start fresh */ - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - - return set_mission_items(); -} - -bool -Navigator::advance_mission() -{ - /* tell mission to move by one */ - _mission.move_to_next(); - - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - return set_mission_items(); -} - -bool -Navigator::set_mission_items() -{ - if (_pos_sp_triplet.current.valid) { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.previous.valid = true; - } - - bool onboard; - int index; - - /* if we fail to set the current mission, continue to loiter */ - if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - - return false; - } - - /* if we got an RTL mission item, switch to RTL mode and give up */ - if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - return false; - } - - _mission_item_valid = true; - - /* convert the current mission item and set it valid */ - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - - mission_item_s next_mission_item; - - bool last_wp = false; - /* now try to set the next mission item as well, if there is no more next - * this means we're heading to the last waypoint */ - if (_mission.get_next_mission_item(&next_mission_item)) { - /* convert the next mission item and set it valid */ - mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); - _pos_sp_triplet.next.valid = true; - } else { - last_wp = true; - } - - /* notify user about what happened */ - mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", - (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - - _update_triplet = true; - - reset_reached(); - - return true; -} - -bool -Navigator::start_rtl() -{ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - /* if RTL doesn't work, fallback to loiter */ - return false; -} - -bool -Navigator::advance_rtl() -{ - /* tell mission to move by one */ - _rtl.move_to_next(); - /* now try to set the new mission items, if it fails, it will dispatch loiter */ - if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - _pos_sp_triplet.current.valid = true; - - reset_reached(); - - _update_triplet = true; - return true; - } - - return false; -} - -bool -Navigator::start_land() -{ - /* TODO: verify/test */ - - reset_reached(); - - /* this state can be requested by commander even if no global position available, - * in his case controller must perform landing without position control */ - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - _mission_item_valid = true; - - mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); - - _pos_sp_triplet.next.valid = false; - - _update_triplet = true; - return true; -} -bool -Navigator::check_mission_item_reached() -{ - /* only check if there is actually a mission item to check */ - if (!_mission_item_valid) { - return false; - } - - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; - } - - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - // return false; - // } - - uint64_t now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - float acceptance_radius; - - if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - if (_do_takeoff) { - /* require only altitude for takeoff */ - if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { - _waypoint_position_reached = true; - } - } else { - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - - /* TODO: removed takeoff, why? */ - if (_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 - _global_pos.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 >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - return true; - } - } - return false; -} - -void -Navigator::reset_reached() -{ - _time_first_inside_orbit = 0; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - -} -#endif void Navigator::publish_position_setpoint_triplet() { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a2eec85893..c5bb06c3b7 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -79,7 +79,7 @@ RTL::on_inactive() /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _rtl_state = RTL_STATE_NONE; } } @@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; if (_first_run) { - _first_run = false; - - /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_FINISHED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else 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; - } - } + _first_run = false; + + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else 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(pos_sp_triplet); @@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* make sure we have the latest params */ updateParams(); - if (_rtl_state == RTL_STATE_FINISHED) { - /* RTL finished, nothing to do */ - pos_sp_triplet->next.valid = false; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); - return; - } + if (_rtl_state == RTL_STATE_FINISHED) { + /* RTL finished, nothing to do */ + pos_sp_triplet->next.valid = false; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed"); + return; + } - set_previous_pos_setpoint(pos_sp_triplet); - _navigator->set_can_loiter_at_sp(false); + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) break; } - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - reset_mission_item_reached(); - pos_sp_triplet->current.valid = true; - pos_sp_triplet->next.valid = false; + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; } void