/**************************************************************************** * * Copyright (c) 2013-2017 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_main.cpp * * Handles mission items, geo fencing and failsafe navigation behavior. * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier * @author Jean Cyr * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler */ #include "navigator.h" #include #include #include #include #include #include #include #include #include #include #include /** * navigator app start / stop handling function * * @ingroup apps */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); #define GEOFENCE_CHECK_INTERVAL 200000 using namespace time_literals; namespace navigator { Navigator *g_navigator; } Navigator::Navigator() : ModuleParams(nullptr), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence(this), _mission(this), _loiter(this), _takeoff(this), _land(this), _precland(this), _rtl(this), _rcLoss(this), _dataLinkLoss(this), _engineFailure(this), _gpsFailure(this), _follow_target(this) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_dataLinkLoss; _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; _navigation_mode_array[8] = &_land; _navigation_mode_array[9] = &_precland; _navigation_mode_array[10] = &_follow_target; _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); _handle_reverse_delay = param_find("VT_B_REV_DEL"); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); reset_triplets(); } Navigator::~Navigator() { orb_unsubscribe(_local_pos_sub); } void Navigator::params_update() { updateParams(); if (_handle_back_trans_dec_mss != PARAM_INVALID) { param_get(_handle_back_trans_dec_mss, &_param_back_trans_dec_mss); } if (_handle_reverse_delay != PARAM_INVALID) { param_get(_handle_reverse_delay, &_param_reverse_delay); } } void Navigator::run() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME); _geofence.loadFromFile(GEOFENCE_FILENAME); } params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); hrt_abstime last_geofence_check = 0; while (!should_exit()) { /* wait for up to 1000ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("poll error %d, %d", pret, errno); px4_usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, local pos is available */ orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } } perf_begin(_loop_perf); /* gps updated */ if (_gps_pos_sub.updated()) { _gps_pos_sub.copy(&_gps_pos); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* global position updated */ if (_global_pos_sub.updated()) { _global_pos_sub.copy(&_global_pos); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } // check for parameter updates if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); // update parameters from storage params_update(); } _vstatus_sub.update(&_vstatus); _land_detected_sub.update(&_land_detected); _position_controller_status_sub.update(); _home_pos_sub.update(&_home_pos); if (_vehicle_command_sub.updated()) { vehicle_command_s cmd{}; _vehicle_command_sub.copy(&cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { // DO_GO_AROUND is currently handled by the position controller (unacknowledged) // TODO: move DO_GO_AROUND handling to navigator publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { position_setpoint_triplet_s *rep = get_reposition_triplet(); position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // If no argument for ground speed, use default value. if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { rep->current.cruising_speed = get_cruising_speed(); } else { rep->current.cruising_speed = cmd.param1; } rep->current.cruising_throttle = get_cruising_throttle(); rep->current.acceptance_radius = get_acceptance_radius(); // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; rep->current.yaw_valid = true; } else { rep->current.yaw = NAN; rep->current.yaw_valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { // Position change with optional altitude change rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } } else if (PX4_ISFINITE(cmd.param7) && curr->current.valid && PX4_ISFINITE(curr->current.lat) && PX4_ISFINITE(curr->current.lon)) { // Altitude without position change rep->current.lat = curr->current.lat; rep->current.lon = curr->current.lon; rep->current.alt = cmd.param7; } else { // All three set to NaN - hold in current position rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; // CMD_DO_REPOSITION is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; if (home_position_valid()) { rep->current.yaw = cmd.param4; rep->previous.valid = true; } else { rep->current.yaw = get_local_position()->yaw; rep->previous.valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = (double)NAN; rep->current.lon = (double)NAN; } rep->current.alt = cmd.param7; rep->current.valid = true; rep->next.valid = false; // CMD_NAV_TAKEOFF is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ if (_mission.land_start()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); publish_vehicle_cmd(&vcmd); } else { PX4_WARN("planned mission landing not available"); } publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { if (!_mission.set_current_mission_index(cmd.param1)) { PX4_WARN("CMD_MISSION_START failed"); } } // CMD_MISSION_START is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { if (cmd.param2 > FLT_EPSILON) { // XXX not differentiating ground and airspeed yet set_cruising_speed(cmd.param2); } else { set_cruising_speed(); /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { set_cruising_throttle(cmd.param3 / 100); } else { set_cruising_throttle(); } } // TODO: handle responses for supported DO_CHANGE_SPEED options? publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) { _vroi = {}; switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: case vehicle_command_s::VEHICLE_CMD_NAV_ROI: _vroi.mode = cmd.param1; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: _vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION; _vroi.lat = cmd.param5; _vroi.lon = cmd.param6; _vroi.alt = cmd.param7; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: _vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT; _vroi.pitch_offset = (float)cmd.param5 * M_DEG_TO_RAD_F; _vroi.roll_offset = (float)cmd.param6 * M_DEG_TO_RAD_F; _vroi.yaw_offset = (float)cmd.param7 * M_DEG_TO_RAD_F; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; break; default: _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; break; } _vroi.timestamp = hrt_absolute_time(); _vehicle_roi_pub.publish(_vroi); publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } } /* Check for traffic */ check_traffic(); /* Check geofence violation */ if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.check(_global_pos, _gps_pos, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); _geofence_result.home_required = _geofence.isHomeRequired(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } _geofence_result_pub.publish(_geofence_result); } /* Do stuff according to navigation state set by commander */ NavigatorMode *navigation_mode_new{nullptr}; switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_rcLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { _pos_sp_triplet_published_invalid_once = false; const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; switch (rtl_type()) { case RTL::RTL_LAND: if (rtl_activated) { mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated"); } // if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly if (on_mission_landing() && !get_land_detected()->landed) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); navigation_mode_new = &_mission; } else { navigation_mode_new = &_rtl; } break; case RTL::RTL_MISSION: if (_mission.get_land_start_available() && !get_land_detected()->landed) { // the mission contains a landing spot _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); if (_navigation_mode != &_mission) { if (_navigation_mode == nullptr) { // switching from an manual mode, go to landing if not already landing if (!on_mission_landing()) { start_mission_landing(); } } else { // switching from an auto mode, continue the mission from the closest item _mission.set_closest_item_as_current(); } } if (rtl_activated) { mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission"); } navigation_mode_new = &_mission; } else { // fly the mission in reverse if switching from a non-manual mode _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && (! _mission.get_mission_finished()) && (!get_land_detected()->landed)) { // determine the closest mission item if switching from a non-mission mode, and we are either not already // mission mode or the mission waypoints changed. // The seconds condition is required so that when no mission was uploaded and one is available the closest // mission item is determined and also that if the user changes the active mission index while rtl is active // always that waypoint is tracked first. if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) { _mission.set_closest_item_as_current(); } if (rtl_activated) { mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse"); } navigation_mode_new = &_mission; } else { if (rtl_activated) { mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home"); } navigation_mode_new = &_rtl; } } break; default: if (rtl_activated) { mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL HOME activated"); } navigation_mode_new = &_rtl; break; } break; } case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_precland; _precland.set_mode(PrecLandMode::Required); break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_dataLinkLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_follow_target; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = nullptr; _can_loiter_at_sp = false; break; } // update the vehicle status _previous_nav_state = _vstatus.nav_state; /* we have a new navigation mode: reset triplet */ if (_navigation_mode != navigation_mode_new) { // We don't reset the triplet if we just did an auto-takeoff and are now // going to loiter. Otherwise, we lose the takeoff altitude and end up lower // than where we wanted to go. // // FIXME: a better solution would be to add reset where they are needed and remove // this general reset here. if (!(_navigation_mode == &_takeoff && navigation_mode_new == &_loiter)) { reset_triplets(); } } _navigation_mode = navigation_mode_new; /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if we landed and have not received takeoff setpoint then stay in idle */ if (_land_detected.landed && !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) { _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.next.valid = false; } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; reset_triplets(); } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); } if (_mission_result_updated) { publish_mission_result(); } perf_end(_loop_perf); } } int Navigator::task_spawn(int argc, char *argv[]) { _task_id = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_NAVIGATION, 1800, (px4_main_t)&run_trampoline, (char *const *)argv); if (_task_id < 0) { _task_id = -1; return -errno; } return 0; } Navigator *Navigator::instantiate(int argc, char *argv[]) { Navigator *instance = new Navigator(); if (instance == nullptr) { PX4_ERR("alloc failed"); } return instance; } int Navigator::print_status() { PX4_INFO("Running"); _geofence.printStatus(); return 0; } void Navigator::publish_position_setpoint_triplet() { // do not publish an invalid setpoint if (!_pos_sp_triplet.current.valid) { return; } _pos_sp_triplet.timestamp = hrt_absolute_time(); /* lazily publish the position setpoint triplet only once available */ _pos_sp_triplet_pub.publish(_pos_sp_triplet); _pos_sp_triplet_updated = false; } float Navigator::get_default_acceptance_radius() { return _param_nav_acc_rad.get(); } float Navigator::get_acceptance_radius() { return get_acceptance_radius(_param_nav_acc_rad.get()); } float Navigator::get_default_altitude_acceptance_radius() { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return _param_nav_fw_alt_rad.get(); } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { return INFINITY; } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; } } float Navigator::get_altitude_acceptance_radius() { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { // Use separate (tighter) altitude acceptance for clean altitude starting point before landing return _param_nav_fw_altl_rad.get(); } } return get_default_altitude_acceptance_radius(); } float Navigator::get_cruising_speed() { /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) { return _mission_cruising_speed_mc; } else { return -1.0f; } } else { if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) { return _mission_cruising_speed_fw; } else { return -1.0f; } } } void Navigator::set_cruising_speed(float speed) { if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _mission_cruising_speed_mc = speed; } else { _mission_cruising_speed_fw = speed; } } void Navigator::reset_cruising_speed() { _mission_cruising_speed_mc = -1.0f; _mission_cruising_speed_fw = -1.0f; } void Navigator::reset_triplets() { _pos_sp_triplet.current.valid = false; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } float Navigator::get_cruising_throttle() { /* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */ if (_mission_throttle > FLT_EPSILON) { return _mission_throttle; } else { return -1.0f; } } float Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; // XXX only use navigation capabilities for now // when in fixed wing mode // this might need locking against a commanded transition // so that a stale _vstatus doesn't trigger an accepted mission item. const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && pos_ctrl_status.acceptance_radius > radius) { radius = pos_ctrl_status.acceptance_radius; } return radius; } float Navigator::get_yaw_acceptance(float mission_item_yaw) { float yaw = mission_item_yaw; const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that // the waypoint can be reached from any direction if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { yaw = pos_ctrl_status.yaw_acceptance; } return yaw; } void Navigator::load_fence_from_file(const char *filename) { _geofence.loadFromFile(filename); } /** * Creates a fake traffic measurement with supplied parameters. * */ void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff, float hor_velocity, float ver_velocity) { double lat, lon; waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat, &lon); float alt = get_global_position()->alt + altitude_diff; // float vel_n = get_global_position()->vel_n; // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; transponder_report_s tr{}; tr.timestamp = hrt_absolute_time(); tr.icao_address = 1234; tr.lat = lat; // Latitude, expressed as degrees tr.lon = lon; // Longitude, expressed as degrees tr.altitude_type = 0; tr.altitude = alt; tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1); tr.callsign[sizeof(tr.callsign) - 1] = 0; tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum tr.tslc = 2; // Time since last communication in seconds tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields tr.squawk = 6667; uORB::PublicationQueued tr_pub{ORB_ID(transponder_report)}; tr_pub.publish(tr); } void Navigator::check_traffic() { double lat = get_global_position()->lat; double lon = get_global_position()->lon; float alt = get_global_position()->alt; // TODO for non-multirotors predicting the future // position as accurately as possible will become relevant // float vel_n = get_global_position()->vel_n; // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; bool changed = _traffic_sub.updated(); float horizontal_separation = 500; float vertical_separation = 500; while (changed) { transponder_report_s tr{}; _traffic_sub.copy(&tr); uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; if ((tr.flags & required_flags) != required_flags) { changed = _traffic_sub.updated(); continue; } float d_hor, d_vert; get_distance_to_point_global_wgs84(lat, lon, alt, tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert); // predict final altitude (positive is up) in prediction time frame float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity; // Predict until the vehicle would have passed this system at its current speed float prediction_distance = d_hor + 1000.0f; // If the altitude is not getting close to us, do not calculate // the horizontal separation. // Since commercial flights do most of the time keep flight levels // check for the current and for the predicted flight level. // we also make the implicit assumption that this system is on the lowest // flight level close to ground in the // (end_alt - horizontal_separation < alt) condition. If this system should // ever be used in normal airspace this implementation would anyway be // inappropriate as it should be replaced with a TCAS compliant solution. if ((fabsf(alt - tr.altitude) < vertical_separation) || ((end_alt - horizontal_separation) < alt)) { double end_lat, end_lon; waypoint_from_heading_and_distance(tr.lat, tr.lon, tr.heading, prediction_distance, &end_lat, &end_lon); struct crosstrack_error_s cr; if (!get_distance_to_line(&cr, lat, lon, tr.lat, tr.lon, end_lat, end_lon)) { if (!cr.past_end && (fabsf(cr.distance) < horizontal_separation)) { // direction of traffic in human-readable 0..360 degree in earth frame int traffic_direction = math::degrees(tr.heading) + 180; switch (_param_nav_traff_avoid.get()) { case 0: { /* ignore */ PX4_WARN("TRAFFIC %s, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); break; } case 1: { mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); break; } case 2: { mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); // set the return altitude to minimum _rtl.set_return_alt_min(true); // ask the commander to execute an RTL vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; publish_vehicle_cmd(&vcmd); break; } } } } } changed = _traffic_sub.updated(); } } bool Navigator::abort_landing() { // only abort if currently landing and position controller status updated bool should_abort = false; if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { if (_pos_ctrl_landing_status_sub.updated()) { position_controller_landing_status_s landing_status{}; // landing status from position controller must be newer than navigator's last position setpoint if (_pos_ctrl_landing_status_sub.copy(&landing_status)) { if (landing_status.timestamp > _pos_sp_triplet.timestamp) { should_abort = landing_status.abort_landing; } } } } return should_abort; } bool Navigator::force_vtol() { return _vstatus.is_vtol && (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw) && _param_nav_force_vt.get(); } int Navigator::custom_command(int argc, char *argv[]) { if (!is_running()) { print_usage("not running"); return 1; } if (!strcmp(argv[0], "fencefile")) { get_instance()->load_fence_from_file(GEOFENCE_FILENAME); return 0; } else if (!strcmp(argv[0], "fake_traffic")) { get_instance()->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f); get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f); get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f); return 0; } return print_usage("unknown command"); } int navigator_main(int argc, char *argv[]) { return Navigator::main(argc, argv); } void Navigator::publish_mission_result() { _mission_result.timestamp = hrt_absolute_time(); /* lazily publish the mission result only once available */ _mission_result_pub.publish(_mission_result); /* reset some of the flags */ _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; _mission_result_updated = false; } void Navigator::set_mission_failure(const char *reason) { if (!_mission_result.failure) { _mission_result.failure = true; set_mission_result_updated(); mavlink_log_critical(&_mavlink_log_pub, "%s", reason); } } void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); vcmd->source_system = _vstatus.system_id; vcmd->source_component = _vstatus.component_id; vcmd->target_system = _vstatus.system_id; vcmd->confirmation = false; vcmd->from_external = false; // The camera commands are not processed on the autopilot but will be // sent to the mavlink links to other components. switch (vcmd->command) { case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: vcmd->target_component = 100; // MAV_COMP_ID_CAMERA break; default: vcmd->target_component = _vstatus.component_id; break; } _vehicle_cmd_pub.publish(*vcmd); } void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result) { vehicle_command_ack_s command_ack = {}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; command_ack.from_external = false; command_ack.result = result; command_ack.result_param1 = 0; command_ack.result_param2 = 0; _vehicle_cmd_ack_pub.publish(command_ack); } int Navigator::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); } PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description Module that is responsible for autonomous flight modes. This includes missions (read from dataman), takeoff and RTL. It is also responsible for geofence violation checking. ### Implementation The different internal modes are implemented as separate classes that inherit from a common base class `NavigatorMode`. The member `_navigation_mode` contains the current active mode. Navigator publishes position setpoint triplets (`position_setpoint_triplet_s`), which are then used by the position controller. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("navigator", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt"); PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 3 fake transponder_report_s uORB messages"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; }