From 4d6749edc2f226425f28a2fdfd5bbeb277a2cd12 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 26 May 2020 17:26:08 +0200 Subject: [PATCH] mission_block: change loiter handling logic - add ability of "heading wait" for NAV_CMD_LOITER_TIME_LIMIT - For both LOITER_TIME and LOITER_TO_ALT in fixed-wing flight, unify logic: --> reach position --> start loitering --> reach altitude --> start timer (if applicable) --> reach exit heading (if applicable) --> declare mission item reached Signed-off-by: Silvan Fuhrer --- src/modules/mavlink/mavlink_mission.cpp | 1 + src/modules/navigator/mission_block.cpp | 132 +++++++++++++----------- src/modules/navigator/mission_block.h | 2 - 3 files changed, 74 insertions(+), 61 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 09c7e1d724..7de65addc3 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1640,6 +1640,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_LOITER_TIME_LIMIT: mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->param2 = mission_item->force_heading; mavlink_mission_item->param3 = mission_item->loiter_radius; mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack; break; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index accac076e5..004795fbf8 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -141,9 +141,7 @@ MissionBlock::is_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - float mission_item_altitude_amsl = _mission_item.altitude_is_relative - ? _mission_item.altitude + _navigator->get_home_position()->alt - : _mission_item.altitude; + const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item); dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, _navigator->get_global_position()->lat, @@ -199,27 +197,20 @@ MissionBlock::is_mission_item_reached() * Therefore the item is marked as reached once the system reaches the loiter * radius + L1 distance. Time inside and turn count is handled elsewhere. */ - float radius = (fabsf(_mission_item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); // check if within loiter radius around wp, if yes then set altitude sp to mission item if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; - - } else { - _time_first_inside_orbit = 0; } } else if (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - - // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter - // first check if the altitude setpoint is the mission setpoint - position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter. + // First check if the altitude setpoint is the mission setpoint (that means that the loiter is not yet reached) + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; if (fabsf(curr_sp->alt - mission_item_altitude_amsl) >= FLT_EPSILON) { - // check if the initial loiter has been accepted dist_xy = -1.0f; dist_z = -1.0f; @@ -233,31 +224,16 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) && dist_z <= _navigator->get_default_altitude_acceptance_radius()) { - // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = mission_item_altitude_amsl; + curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; _navigator->set_position_setpoint_triplet_updated(); } } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) && dist_z <= _navigator->get_altitude_acceptance_radius()) { + // loitering, check if new altitude is reached, while still also having check on position _waypoint_position_reached = true; - - // set required yaw from bearing to the next mission item - if (_mission_item.force_heading) { - const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; - - if (next_sp.valid) { - _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - next_sp.lat, next_sp.lon); - - _waypoint_yaw_reached = false; - - } else { - _waypoint_yaw_reached = true; - } - } } } else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { @@ -327,29 +303,25 @@ MissionBlock::is_mission_item_reached() } } - if (_waypoint_position_reached && !_waypoint_position_reached_previously) { + if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } + + // consider yaw reached for non-rotary wing vehicles (such as fixed-wing) + if (_navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _waypoint_yaw_reached = true; + } } - /* Check if the waypoint and the requested yaw setpoint. */ + /* Check if the requested yaw setpoint is reached (only for rotary wing flight). */ + if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) - || ((_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) && _mission_item.force_heading - && PX4_ISFINITE(_mission_item.yaw))) { + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) { - /* check course if defined only for rotary wing except takeoff */ - float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ? - _navigator->get_local_position()->heading : - atan2f( - _navigator->get_local_position()->vy, - _navigator->get_local_position()->vx - ); - - float yaw_err = wrap_pi(_mission_item.yaw - cog); + const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < _navigator->get_yaw_threshold() @@ -374,14 +346,62 @@ MissionBlock::is_mission_item_reached() /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { - if (_time_first_inside_orbit == 0) { - _time_first_inside_orbit = now; - } + bool time_inside_reached = false; /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + time_inside_reached = true; + } + // check if heading for exit is reached (only applies for fixed-wing flight) + bool exit_heading_reached = false; + + if (time_inside_reached) { + + struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current; + const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; + + /* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set, + or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */ + const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && + next_sp.valid && + curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && + (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT); + + if (enforce_exit_heading) { + + + const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat, + next_sp.lon); + + float yaw_err = 0.0f; + + if (dist_current_next > 1.2f * _navigator->get_loiter_radius()) { + // set required yaw from bearing to the next mission item + _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + next_sp.lat, next_sp.lon); + const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx); + yaw_err = wrap_pi(_mission_item.yaw - cog); + + + + } + + + if (fabsf(yaw_err) < 0.1f) { //accept heading for exit if below 0.1 rad error (5.7deg) + exit_heading_reached = true; + } + + } else { + exit_heading_reached = true; + } + } + + // set exit flight course to next waypoint + if (exit_heading_reached) { position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; @@ -412,14 +432,10 @@ MissionBlock::is_mission_item_reached() &curr_sp.lat, &curr_sp.lon); } - return true; + return true; // mission item is reached } } - // all acceptance criteria must be met in the same iteration - _waypoint_position_reached_previously = _waypoint_position_reached; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; return false; } @@ -428,7 +444,6 @@ MissionBlock::reset_mission_item_reached() { _waypoint_position_reached = false; _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; _time_wp_reached = 0; } @@ -586,7 +601,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi case NAV_CMD_LOITER_TO_ALT: // initially use current altitude, and switch to mission item altitude once in loiter position - if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 (-1) + if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0 sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); @@ -594,11 +609,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->alt = _navigator->get_global_position()->alt; } - // fall through - case NAV_CMD_LOITER_TIME_LIMIT: - // FALLTHROUGH + case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_UNLIMITED: + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 0e504230f5..b432e9f2cd 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -148,9 +148,7 @@ protected: bool _waypoint_position_reached{false}; bool _waypoint_yaw_reached{false}; - bool _waypoint_position_reached_previously{false}; - hrt_abstime _time_first_inside_orbit{0}; hrt_abstime _action_start{0}; hrt_abstime _time_wp_reached{0};