/**************************************************************************** * * Copyright (c) 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 mission_block.cpp * * Helper class to use mission items * * @author Julian Oes * @author Sander Smeets * @author Andreas Antener */ #include "mission_block.h" #include "navigator.h" #include #include #include #include #include #include #include #include using matrix::wrap_pi; MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : NavigatorMode(navigator, navigator_state_id) { } bool MissionBlock::is_mission_item_reached_or_completed() { const hrt_abstime now = hrt_absolute_time(); // Handle indefinite waypoints and action commands switch (_mission_item.nav_cmd) { // Action Commands that doesn't have timeout completes instantaneously case NAV_CMD_DO_SET_ACTUATOR: case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_TRIGGER_CONTROL: case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: case NAV_CMD_DO_SET_ROI_NONE: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_CHANGE_SPEED: case NAV_CMD_DO_SET_HOME: return true; // Indefinite Waypoints case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_VTOL_TRANSITION: if (int(_mission_item.params[0]) == 3) { // transition to RW requested, only accept waypoint if vehicle state has changed accordingly return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_vstatus()->in_transition_mode; } else if (int(_mission_item.params[0]) == 4) { // transition to FW requested, only accept waypoint if vehicle state has changed accordingly return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; } else { // invalid vtol transition request return false; } case NAV_CMD_VTOL_TAKEOFF: if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return true; } break; case NAV_CMD_DELAY: // Set reached flags directly such that only the delay time is considered _waypoint_position_reached = true; _waypoint_yaw_reached = true; // Set timestamp when entering only (it's reset to 0 for every waypoint) if (_time_wp_reached == 0) { _time_wp_reached = now; } break; case NAV_CMD_DO_WINCH: { const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; // TODO: Add proper microseconds_to_seconds function if (_payload_deploy_ack_successful) { PX4_DEBUG("Winch has acknowledged, resume mission"); return true; } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { PX4_DEBUG("Winch timed out, resume mission"); return true; } // We are still waiting for the acknowledgement / execution of deploy return false; } case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: { const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f; if (gimbal_command_elasped_time_s > _gimbal_wait_time) { PX4_DEBUG("Gimbal aligned, resume mission"); return true; } // We are still waiting the desired delay for the gimbal return false; } case NAV_CMD_DO_GRIPPER: { const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; if (_payload_deploy_ack_successful) { PX4_DEBUG("Gripper has acknowledged, resume mission"); return true; } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { PX4_DEBUG("Gripper timed out, resume mission"); return true; } // We are still waiting for the acknowledgement / execution of deploy return false; } default: /* do nothing, this is a 3D waypoint */ break; } // Update the 'waypoint position reached' status if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item); // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : _navigator->get_loiter_radius(); dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_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 || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : (_mission_item.altitude - _navigator->get_home_position()->alt); float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { altitude_acceptance_radius = takeoff_alt / 2.0f; } /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > mission_item_altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { /* fixed-wing takeoff is reached once the vehicle has exceeded the takeoff altitude */ if (_navigator->get_global_position()->alt > mission_item_altitude_amsl) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { // Accept takeoff waypoint to be reached if the distance in 2D plane is within acceptance radius if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { // For takeoff mission items use the parameter for the takeoff acceptance radius if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius + navigation switch distance. Time inside and turn count is handled elsewhere. */ // 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() + mission_item_loiter_radius_abs) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } 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 (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) { dist_xy = -1.0f; dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); // 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() + mission_item_loiter_radius_abs) && dist_z <= _navigator->get_altitude_acceptance_radius()) { 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() + mission_item_loiter_radius_abs) && 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; } } else if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; // if the setpoint is valid we are checking if we reached the gate // in the case of an invalid setpoint we are defaulting to // assuming that we have already reached the gate to not block // the further execution of the mission. if (curr_sp->valid) { // location of gate (mission item) MapProjection ref_pos{_mission_item.lat, _mission_item.lon}; // current setpoint matrix::Vector2f gate_to_curr_sp = ref_pos.project(curr_sp->lat, curr_sp->lon); // system position matrix::Vector2f vehicle_pos = ref_pos.project(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon); const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized()); // if the dot product (projected vector) is positive, then // the current position is between the gate position and the // next waypoint if (dot_product >= 0) { _waypoint_position_reached = true; _waypoint_yaw_reached = true; _time_wp_reached = now; } } } else { float acceptance_radius = _navigator->get_acceptance_radius(); // We use the acceptance radius of the mission item if it has been set (not NAN) // but only for multicopter. if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && PX4_ISFINITE(_mission_item.acceptance_radius) && _mission_item.acceptance_radius > FLT_EPSILON) { acceptance_radius = _mission_item.acceptance_radius; } float alt_acc_rad_m = _navigator->get_altitude_acceptance_radius(); /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration(); if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) { acceptance_radius = (velocity / back_trans_dec / 2) * velocity; } // do not care for altitude when approaching the backtransition point. Not accepting the waypoint causes // the vehicle to perform a sharp turn after passing the land waypoint and this causes worse unexected behavior alt_acc_rad_m = INFINITY; } bool passed_curr_wp = false; if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { const float dist_prev_to_curr = get_distance_to_next_waypoint(_navigator->get_position_setpoint_triplet()->previous.lat, _navigator->get_position_setpoint_triplet()->previous.lon, _navigator->get_position_setpoint_triplet()->current.lat, _navigator->get_position_setpoint_triplet()->current.lon); if (dist_prev_to_curr > 1.0e-6f && _navigator->get_position_setpoint_triplet()->previous.valid) { // Fixed-wing guidance interprets this condition as line segment following // vector from previous waypoint to current waypoint float vector_prev_to_curr_north; float vector_prev_to_curr_east; get_vector_to_next_waypoint_fast(_navigator->get_position_setpoint_triplet()->previous.lat, _navigator->get_position_setpoint_triplet()->previous.lon, _navigator->get_position_setpoint_triplet()->current.lat, _navigator->get_position_setpoint_triplet()->current.lon, &vector_prev_to_curr_north, &vector_prev_to_curr_east); // vector from next waypoint to aircraft float vector_curr_to_vehicle_north; float vector_curr_to_vehicle_east; get_vector_to_next_waypoint_fast(_navigator->get_position_setpoint_triplet()->current.lat, _navigator->get_position_setpoint_triplet()->current.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, &vector_curr_to_vehicle_north, &vector_curr_to_vehicle_east); // if dot product of vectors is positive, we are passed the current waypoint (the terminal point on the line segment) and should switch to next mission item passed_curr_wp = vector_prev_to_curr_north * vector_curr_to_vehicle_north + vector_prev_to_curr_east * vector_curr_to_vehicle_east > 0.0f; } } if (dist_xy >= 0.0f && (dist_xy <= acceptance_radius || passed_curr_wp) && dist_z <= alt_acc_rad_m) { _waypoint_position_reached = true; } } 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; } } // Update the 'waypoint position reached' status (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 && _navigator->get_yaw_to_be_accepted(_mission_item.yaw) && _navigator->get_local_position()->heading_good_for_control) { 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() || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_navigator->get_yaw_timeout() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) { _navigator->set_mission_failure_heading_timeout(); } } else { _waypoint_yaw_reached = true; } } // Handle Loiter/Delay Timeout if the waypoint position and yaw setpoint got reached if (_waypoint_position_reached && _waypoint_yaw_reached) { 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_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { time_inside_reached = true; } // check if course for exit is reached (only applies for fixed-wing flight) bool exit_course_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; const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat, next_sp.lon); /* enforce exit course 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_course = _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); // can only enforce exit course if next waypoint is not within loiter radius of current waypoint (with small margin) const bool exit_course_is_reachable = dist_current_next > 1.05f * curr_sp_new->loiter_radius; if (enforce_exit_course && exit_course_is_reachable) { float vehicle_position_to_next_waypoint_north; float vehicle_position_to_next_waypoint_east; get_vector_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat, next_sp.lon, &vehicle_position_to_next_waypoint_north, &vehicle_position_to_next_waypoint_east); // this vector defines the exit bearing const matrix::Vector2f vector_vehicle_position_to_next_waypoint = {vehicle_position_to_next_waypoint_north, vehicle_position_to_next_waypoint_east}; const matrix::Vector2f vehicle_ground_velocity = {_navigator->get_local_position()->vx, _navigator->get_local_position()->vy}; exit_course_reached = vector_vehicle_position_to_next_waypoint.dot(vehicle_ground_velocity) > vector_vehicle_position_to_next_waypoint.norm() * vehicle_ground_velocity.norm() * kCosineExitCourseThreshold; } else { exit_course_reached = true; } } // set exit flight course to next waypoint if (exit_course_reached) { position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); // exit xtrack location // reset lat/lon of loiter waypoint so vehicle follows a tangent if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); // calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location const float ratio = math::min(fabsf(curr_sp.loiter_radius / range), 1.0f); float inner_angle = acosf(ratio); // Compute "ideal" tangent origin if (curr_sp.loiter_direction_counter_clockwise) { bearing += inner_angle; } else { bearing -= inner_angle; } // set typ to position, will get set to loiter in the fw position controller once close // and replace current setpoint lat/lon with tangent coordinate curr_sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION; waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, bearing, fabsf(curr_sp.loiter_radius), &curr_sp.lat, &curr_sp.lon); } return true; // mission item is reached } } return false; } void MissionBlock::reset_mission_item_reached() { _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_wp_reached = 0; } void MissionBlock::issue_command(const mission_item_s &item) { if (item_contains_position(item) || item_contains_gate(item) || item_contains_marker(item)) { return; } if (item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER) { // Initiate Payload Deployment vehicle_command_s vcmd = {}; vcmd.command = item.nav_cmd; vcmd.param1 = item.params[0]; vcmd.param2 = item.params[1]; vcmd.param3 = item.params[2]; vcmd.param4 = item.params[3]; vcmd.param5 = static_cast(item.params[4]); vcmd.param6 = static_cast(item.params[5]); _navigator->publish_vehicle_cmd(&vcmd); // Reset payload deploy flag & data to get ready to receive deployment ack result _payload_deploy_ack_successful = false; _payload_deployed_time = hrt_absolute_time(); } else { // This is to support legacy DO_MOUNT_CONTROL as part of a mission. if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { _navigator->acquire_gimbal_control(); } // Mission item's NAV_CMD enums directly map to the according vehicle command // So set the raw value directly (MAV_FRAME_MISSION mission item) vehicle_command_s vcmd = {}; vcmd.command = item.nav_cmd; vcmd.param1 = item.params[0]; vcmd.param2 = item.params[1]; vcmd.param3 = item.params[2]; vcmd.param4 = item.params[3]; vcmd.param5 = static_cast(item.params[4]); vcmd.param6 = static_cast(item.params[5]); vcmd.param7 = item.params[6]; if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision vcmd.param5 = item.lat; vcmd.param6 = item.lon; if (item.altitude_is_relative) { vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; } } // Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait // for the gimbal to reach the desired position if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { _gimbal_command_time = hrt_absolute_time(); } _navigator->publish_vehicle_cmd(&vcmd); } } float MissionBlock::get_time_inside(const mission_item_s &item) const { if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) || item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || item.nav_cmd == NAV_CMD_DELAY) { // a negative time inside would be invalid return math::max(item.time_inside, 0.0f); } return 0.0f; } // TODO: get_time_inside and item_has_timeout is quite redundant. Separate them out // Problem arises from the fact that DO_WINCH and DO_GRIPPER *should be an instantaneous command, // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER // or so, and have a function named 'item_is_conditional' // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands // A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW bool MissionBlock::item_has_timeout(const mission_item_s &item) { return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER || item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW; } bool MissionBlock::item_contains_position(const mission_item_s &item) { return item.nav_cmd == NAV_CMD_WAYPOINT || item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || item.nav_cmd == NAV_CMD_LAND || item.nav_cmd == NAV_CMD_TAKEOFF || item.nav_cmd == NAV_CMD_LOITER_TO_ALT || item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || item.nav_cmd == NAV_CMD_VTOL_LAND; } bool MissionBlock::item_contains_gate(const mission_item_s &item) { return item.nav_cmd == NAV_CMD_CONDITION_GATE; } bool MissionBlock::item_contains_marker(const mission_item_s &item) { return item.nav_cmd == NAV_CMD_DO_LAND_START; } bool MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp) { // Don't change the setpoint for non-position items if (!item_contains_position(item)) { return false; } sp->lat = item.lat; sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) { // if the mission item has a specified acceptance radius, overwrite the default one from parameters sp->acceptance_radius = item.acceptance_radius; } else { sp->acceptance_radius = _navigator->get_default_acceptance_radius(); } // by default, FW guidance logic will take alt acceptance from NAV_FW_ALT_RAD, in some special cases // we override it after this sp->alt_acceptance_radius = NAN; sp->cruising_speed = _navigator->get_cruising_speed(); sp->cruising_throttle = _navigator->get_cruising_throttle(); // for fixed wing we don't use cruising_throttle directly anymore, instead we command airspeed setpoints via cruising_speed // we still use cruising throttle here to determine if gliding is enabled sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON); switch (item.nav_cmd) { case NAV_CMD_IDLE: sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: case NAV_CMD_VTOL_TAKEOFF: // if already flying (armed and !landed) treat TAKEOFF like regular POSITION if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !_navigator->get_land_detected()->landed && !_navigator->get_land_detected()->maybe_landed) { sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; } else { sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; // Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset. // The yaw setpoint generation is handled by FlightTaskAuto. sp->yaw = NAN; } break; case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TO_ALT: sp->alt = _navigator->get_global_position()->alt; // FALLTHROUGH case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_UNLIMITED: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; default: sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } sp->valid = true; sp->timestamp = hrt_absolute_time(); return sp->valid; } void MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item) { setLoiterItemCommonFields(item); const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); item->lat = pos_sp_triplet->current.lat; item->lon = pos_sp_triplet->current.lon; item->altitude = pos_sp_triplet->current.alt; item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ? -pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius; item->yaw = pos_sp_triplet->current.yaw; } void MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) { setLoiterItemCommonFields(item); item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; // check if minimum loiter altitude is specified, and enforce it if so float loiter_altitude_amsl = _navigator->get_global_position()->alt; if (_navigator->get_loiter_min_alt() > FLT_EPSILON) { loiter_altitude_amsl = math::max(loiter_altitude_amsl, _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt()); } item->altitude = loiter_altitude_amsl; item->loiter_radius = _navigator->get_loiter_radius(); item->yaw = NAN; } void MissionBlock::setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item) { setLoiterItemCommonFields(item); _navigator->preproject_stop_point(item->lat, item->lon); item->altitude = _navigator->get_global_position()->alt; item->loiter_radius = _navigator->get_loiter_radius(); item->yaw = NAN; } void MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item) { item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; item->altitude_is_relative = false; item->acceptance_radius = _navigator->get_acceptance_radius(); item->yaw = NAN; item->time_inside = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } void MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) { item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; item->yaw = NAN; item->altitude = abs_altitude; item->altitude_is_relative = false; item->acceptance_radius = _navigator->get_acceptance_radius(); item->loiter_radius = _navigator->get_loiter_radius(); item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } void MissionBlock::set_land_item(struct mission_item_s *item) { /* VTOL transition to RW before landing */ if (_navigator->force_vtol()) { vehicle_command_s vcmd = {}; vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; vcmd.param2 = 0.0f; _navigator->publish_vehicle_cmd(&vcmd); } /* set the land item */ item->nav_cmd = NAV_CMD_LAND; // set land item to current position if (_navigator->get_local_position()->xy_global) { item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; } else { item->lat = (double)NAN; item->lon = (double)NAN; } item->yaw = NAN; item->altitude = 0; item->altitude_is_relative = false; item->loiter_radius = _navigator->get_loiter_radius(); item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } void MissionBlock::set_idle_item(struct mission_item_s *item) { item->nav_cmd = NAV_CMD_IDLE; item->lat = _navigator->get_home_position()->lat; item->lon = _navigator->get_home_position()->lon; item->altitude_is_relative = false; item->altitude = _navigator->get_home_position()->alt; item->yaw = NAN; item->loiter_radius = _navigator->get_loiter_radius(); item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } void MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode) { item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; item->params[0] = (float) new_mode; item->params[1] = 0.0f; // not immediate transition item->autocontinue = true; } float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const { return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt); } float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt) { if (mission_item.altitude_is_relative) { return mission_item.altitude + home_alt; } else { return mission_item.altitude; } } void MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item, const struct position_setpoint_s *const setpoint) const { if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { mission_item->lat = setpoint->lat; mission_item->lon = setpoint->lon; mission_item->altitude = setpoint->alt; } else { mission_item->lat = _navigator->get_global_position()->lat; mission_item->lon = _navigator->get_global_position()->lon; mission_item->altitude = _navigator->get_global_position()->alt; } mission_item->altitude_is_relative = false; } void MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item, const struct mission_item_s *const mission_item_next) const { mission_item->nav_cmd = NAV_CMD_WAYPOINT; copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); mission_item->altitude_is_relative = false; mission_item->autocontinue = true; mission_item->time_inside = 0.0f; mission_item->yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, mission_item_next->lat, mission_item_next->lon); mission_item->force_heading = true; } void MissionBlock::initialize() { _mission_item.lat = (double)NAN; _mission_item.lon = (double)NAN; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; } void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_radius) const { item.nav_cmd = NAV_CMD_LOITER_TO_ALT; item.lat = pos_yaw_sp.lat; item.lon = pos_yaw_sp.lon; item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; item.origin = ORIGIN_ONBOARD; item.loiter_radius = loiter_radius; } void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_time, float loiter_radius) const { const bool autocontinue = (loiter_time > -FLT_EPSILON); if (autocontinue) { item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; } else { item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; } item.lat = pos_yaw_sp.lat; item.lon = pos_yaw_sp.lon; item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.yaw = NAN; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = math::max(loiter_time, 0.0f); item.autocontinue = autocontinue; item.origin = ORIGIN_ONBOARD; item.loiter_radius = loiter_radius; } void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_WAYPOINT; item.lat = pos_yaw_sp.lat; item.lon = pos_yaw_sp.lon; item.altitude = pos_yaw_sp.alt; item.altitude_is_relative = false; item.autocontinue = true; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.f; item.origin = ORIGIN_ONBOARD; item.yaw = pos_yaw_sp.yaw; } void MissionBlock::setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const { item.nav_cmd = NAV_CMD_LAND; item.lat = pos_yaw_sp.lat; item.lon = pos_yaw_sp.lon; item.altitude = pos_yaw_sp.alt; item.yaw = pos_yaw_sp.yaw; item.acceptance_radius = _navigator->get_acceptance_radius(); item.time_inside = 0.0f; item.autocontinue = true; item.origin = ORIGIN_ONBOARD; } void MissionBlock::startPrecLand(uint16_t land_precision) { if (_mission_item.land_precision == 1) { _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); _navigator->get_precland()->on_activation(); } else { //_mission_item.land_precision == 2 _navigator->get_precland()->set_mode(PrecLandMode::Required); _navigator->get_precland()->on_activation(); } } void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item) { // Avoid flying into terrain using the distance sensor. Enable through the parameter NAV_MIN_GND_DIST. // Only active during commanded descents with vz>0 (to prevent climb-aways), excluding landing and VTOL transitions. // It changes the altitude setpoint in the triplet to maintain the current altitude and republish the triplet. // We also change the mission item altitude used for acceptance calculations to prevent getting stuck in a loop. // This threshold is needed to prevent the check from re-triggering due to small altitude over-shoots while // tracking the new altitude setpoint. static constexpr float kAltitudeDifferenceForDescentCondition = 2.f; if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION && _mission_item.nav_cmd != NAV_CMD_IDLE && _navigator->get_local_position()->dist_bottom_valid && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param() && _navigator->get_local_position()->vz > FLT_EPSILON && _navigator->get_global_position()->alt - get_absolute_altitude_for_item(mission_item) > kAltitudeDifferenceForDescentCondition) { _navigator->sendWarningDescentStoppedDueToTerrain(); struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; curr_sp->alt = _navigator->get_global_position()->alt; _navigator->set_position_setpoint_triplet_updated(); _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; } } void MissionBlock::updateFailsafeChecks() { updateMaxHaglFailsafe(); } void MissionBlock::updateMaxHaglFailsafe() { const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; if (_navigator->get_global_position()->terrain_alt_valid && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); _navigator->trigger_hagl_failsafe(getNavigatorStateId()); // While waiting for a failsafe action from commander, keep the curren position setLoiterItemFromCurrentPosition(&_mission_item); mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); _navigator->set_position_setpoint_triplet_updated(); } }