diff --git a/src/lib/mission/planned_mission_interface.cpp b/src/lib/mission/planned_mission_interface.cpp index bfd445bcf7..9d8453f734 100644 --- a/src/lib/mission/planned_mission_interface.cpp +++ b/src/lib/mission/planned_mission_interface.cpp @@ -277,7 +277,7 @@ int PlannedMissionInterface::setMissionIndex(int32_t index) } } -int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, +int32_t PlannedMissionInterface::getClosestMissionItemIndex(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status) { int32_t min_dist_index(-1); @@ -311,6 +311,14 @@ int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, flo } } + return min_dist_index; +} + +int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status) +{ + int32_t min_dist_index(getClosestMissionItemIndex(lat, lon, alt, home_alt, vehicle_status)); + return goToItem(min_dist_index, false); } diff --git a/src/lib/mission/planned_mission_interface.h b/src/lib/mission/planned_mission_interface.h index 3358adb76a..6b21938731 100644 --- a/src/lib/mission/planned_mission_interface.h +++ b/src/lib/mission/planned_mission_interface.h @@ -64,6 +64,8 @@ public: int goToPreviousPositionItem(bool execute_jump); int goToNextPositionItem(bool execute_jump); int goToMissionLandStart(); + int32_t getClosestMissionItemIndex(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status); int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status); virtual void onMissionUpdate(bool has_mission_items_changed) = 0; @@ -101,11 +103,9 @@ protected: _land_pos{.lat = 0.0, .lon = 0.0, .alt = 0.0f}; - -private: - bool _is_land_start_item_searched{false}; - uORB::Subscription _mission_sub{ORB_ID(mission)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::Publication _mission_pub{ORB_ID(mission)}; +private: + bool _is_land_start_item_searched{false}; }; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 29c456cc35..40e812a5af 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2170,15 +2170,18 @@ FixedwingPositionControl::Run() _position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat) && PX4_ISFINITE(_pos_sp_triplet.previous.lon) - && PX4_ISFINITE(_pos_sp_triplet.previous.alt); + && PX4_ISFINITE(_pos_sp_triplet.previous.alt) + && _pos_sp_triplet.previous.valid; _position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat) && PX4_ISFINITE(_pos_sp_triplet.current.lon) - && PX4_ISFINITE(_pos_sp_triplet.current.alt); + && PX4_ISFINITE(_pos_sp_triplet.current.alt) + && _pos_sp_triplet.current.valid; _position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat) && PX4_ISFINITE(_pos_sp_triplet.next.lon) - && PX4_ISFINITE(_pos_sp_triplet.next.alt); + && PX4_ISFINITE(_pos_sp_triplet.next.alt) + && _pos_sp_triplet.next.valid; // reset the altitude foh (first order hold) logic _min_current_sp_distance_xy = FLT_MAX; diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 41b6bf647a..c0c27916c6 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -43,6 +43,9 @@ px4_add_module( mission.cpp loiter.cpp rtl.cpp + rtl_direct.cpp + rtl_mission_fast.cpp + rtl_mission_fast_reverse.cpp takeoff.cpp land.cpp precland.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7c36e2aaf9..2999ec8a40 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -63,54 +63,55 @@ using namespace time_literals; -Mission::Mission(Navigator *navigator) : +MissionBase::MissionBase(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { - _is_current_planned_mission_item_valid = initMission() == EXIT_SUCCESS; + _is_current_planned_mission_item_valid = (initMission() == EXIT_SUCCESS); } -void Mission::onMissionUpdate(bool has_mission_items_changed) +void MissionBase::onMissionUpdate(bool has_mission_items_changed) { _is_current_planned_mission_item_valid = true; check_mission_valid(); + if (isActive()) { _mission_has_been_activated = true; _navigator->reset_triplets(); update_mission(); set_mission_items(); - } - else - { + + } else { _mission_has_been_activated = false; } } void -Mission::on_inactive() +MissionBase::on_inactive() { PlannedMissionInterface::update(); _land_detected_sub.update(); _vehicle_status_sub.update(); + _global_pos_sub.update(); /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { check_mission_valid(); _initialized_mission_checked = true; } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _land_detected_sub.get().landed) { _need_takeoff = true; } - if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) - { + if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { _system_disarmed_while_inactive = true; } } void -Mission::on_inactivation() +MissionBase::on_inactivation() { setCameraTrigger(false); @@ -133,7 +134,7 @@ Mission::on_inactivation() } void -Mission::on_activation() +MissionBase::on_activation() { /* reset the current mission to the start sequence if needed.*/ checkMissionRestart(); @@ -150,7 +151,7 @@ Mission::on_activation() } void -Mission::on_active() +MissionBase::on_active() { PlannedMissionInterface::update(); _land_detected_sub.update(); @@ -217,60 +218,28 @@ Mission::on_active() } } -bool -Mission::set_current_mission_index(uint16_t index) +void MissionBase::update_mission() { - if (_is_mission_valid && (index < _mission.count)) { - if(goToItem(index, true) != EXIT_SUCCESS) - { - // Keep the old mission index (it was not updated by the interface) and report back. - return false; - } - else - { - _is_current_planned_mission_item_valid = true; - } - - // update mission items if already in active mission - if (isActive()) { - // prevent following "previous - current" line - _navigator->reset_triplets(); - update_mission(); - set_mission_items(); - } - - return true; - } - - return false; -} - -void -Mission::update_mission() -{ - if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_is_mission_valid) - { - if (_mission_type == MissionType::MISSION_TYPE_MISSION) - { + if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_is_mission_valid) { + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { if (_land_detected_sub.get().landed) { /* landed, refusing to take off without a mission */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, refusing takeoff"); + "No valid mission available, refusing takeoff"); + } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, loitering"); + "No valid mission available, loitering"); } } _mission_type = MissionType::MISSION_TYPE_NONE; - } - else - { - if (_mission_type == MissionType::MISSION_TYPE_NONE) - { - mavlink_log_info(_navigator->get_mavlink_log_pub(),"Executing Mission\t"); + + } else { + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing Mission\t"); events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); } @@ -283,19 +252,10 @@ Mission::update_mission() _navigator->reset_vroi(); if (_is_mission_valid) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; - - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; - /* reset work item if new mission has been accepted */ _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; - } - - if (!_is_mission_valid) { + } else { // only warn if the check failed on merit if ((int)_mission.count > 0) { PX4_WARN("mission check failed"); @@ -304,22 +264,30 @@ Mission::update_mission() } } + if (_is_mission_valid) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; + + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; + } + set_mission_result(); } void -Mission::advance_mission() +MissionBase::advance_mission() { /* do not advance mission item if we're processing sub mission work items */ if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_DEFAULT) { return; } - if (_mission_type == MissionType::MISSION_TYPE_MISSION) - { - _is_current_planned_mission_item_valid = (goToNextItem(true) == EXIT_SUCCESS); - if (!_is_current_planned_mission_item_valid) - { + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + _is_current_planned_mission_item_valid = setNextMissionItem(); + + if (!_is_current_planned_mission_item_valid) { // Mission ended if (_land_detected_sub.get().landed) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, landed\t"); @@ -328,7 +296,7 @@ Mission::advance_mission() } else { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_info(_navigator->get_mavlink_log_pub(),"Mission finished, loitering\t"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, loitering\t"); events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); @@ -340,144 +308,159 @@ Mission::advance_mission() } void -Mission::set_mission_items() +MissionBase::set_mission_items() { - /* mission item that comes after current if available */ - constexpr static size_t max_num_next_items{2u}; - mission_item_s next_mission_items[max_num_next_items]; - size_t num_found_items; - - getNextPositionItems(_mission.current_seq + 1, next_mission_items, num_found_items, max_num_next_items); - if (_is_current_planned_mission_item_valid) { /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ _mission_item = _current_planned_mission_item; - - setMissionSetpoint(next_mission_items, num_found_items); + setActiveMissionItems(); } else { - setEndOfMissionSetpoint(); + setEndOfMissionItems(); } } -bool -Mission::do_need_vertical_takeoff() +void MissionBase::setEndOfMissionItems() { - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - if (_land_detected_sub.get().landed) { - /* force takeoff if landed (additional protection) */ - _need_takeoff = true; - - } else if (_global_pos_sub.get().alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { - /* if in-air and already above takeoff height, don't do takeoff */ - _need_takeoff = false; - - } else if (_global_pos_sub.get().alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() - && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { - /* if in-air but below takeoff height and we have a takeoff item */ - _need_takeoff = true; - } - - /* check if current mission item is one that requires takeoff before */ - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { - - _need_takeoff = false; - return true; - } - } - - return false; -} - -bool -Mission::do_need_move_to_land() -{ - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { - - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - } - - return false; -} - -bool -Mission::do_need_move_to_takeoff() -{ - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { - - float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _global_pos_sub.get().lat, _global_pos_sub.get().lon); - - return d_current > _navigator->get_acceptance_radius(); - } - - return false; -} - -void -Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) -{ - 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 = _global_pos_sub.get().lat; - mission_item->lon = _global_pos_sub.get().lon; - mission_item->altitude = _global_pos_sub.get().alt; - } - - mission_item->altitude_is_relative = false; -} - -void -Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) -{ - 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( - _global_pos_sub.get().lat, _global_pos_sub.get().lon, - mission_item_next->lat, mission_item_next->lon); - mission_item->force_heading = true; -} - -float -Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) -{ - /* calculate takeoff altitude */ - float takeoff_alt = get_absolute_altitude_for_item(*mission_item); - - /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_land_detected_sub.get().landed) { - takeoff_alt = fmaxf(takeoff_alt, _global_pos_sub.get().alt + _navigator->get_takeoff_min_alt()); + _mission_item.nav_cmd = NAV_CMD_IDLE; } else { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + + } else { + setLoiterItemFromCurrentPosition(&_mission_item); + } + } - return takeoff_alt; + /* update position setpoint triplet */ + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + /* reuse setpoint for LOITER only if it's not IDLE */ + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + // set mission finished + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + _mission_type = MissionType::MISSION_TYPE_NONE; } void -Mission::heading_sp_update() +MissionBase::check_mission_valid() +{ + + MissionFeasibilityChecker missionFeasibilityChecker(_navigator); + + _is_mission_valid = + missionFeasibilityChecker.checkMissionFeasible(_mission, + _param_mis_dist_1wp.get(), + _param_mis_dist_wps.get()); + + _navigator->get_mission_result()->valid = _is_mission_valid; + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); +} + +void +MissionBase::set_mission_item_reached() +{ + _navigator->get_mission_result()->seq_reached = _mission.current_seq; + _navigator->set_mission_result_updated(); + + reset_mission_item_reached(); +} + +void +MissionBase::set_mission_result() +{ + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _mission.current_seq; + + _navigator->set_mission_result_updated(); +} + +bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const +{ + return ((p1->valid == p2->valid) && + (p1->type == p2->type) && + (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && + (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && + (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && + (fabs(p1->lat - p2->lat) < DBL_EPSILON) && + (fabs(p1->lon - p2->lon) < DBL_EPSILON) && + (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && + ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && + (p1->yaw_valid == p2->yaw_valid) && + (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && + (p1->yawspeed_valid == p2->yawspeed_valid) && + (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && + (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && + (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && + (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); + +} + +void +MissionBase::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + + _navigator->set_mission_result_updated(); +} + +void MissionBase::setCameraTrigger(bool enable) +{ + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + + if (enable) { + cmd.param3 = 0.0f; + + } else { + cmd.param3 = 1.0f; + } + + _navigator->publish_vehicle_cmd(&cmd); +} + +void +MissionBase::checkMissionRestart() +{ + if (_system_disarmed_while_inactive && _mission_has_been_activated) { + if (goToItem(0u, true) == EXIT_SUCCESS) { + _is_current_planned_mission_item_valid = true; + resetMissionJumpCounter(); + _navigator->reset_cruising_speed(); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t"); + events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission"); + resetMission(); + } + } +} + +void +MissionBase::heading_sp_update() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -553,7 +536,7 @@ Mission::heading_sp_update() } void -Mission::cruising_speed_sp_update() +MissionBase::cruising_speed_sp_update() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -572,11 +555,10 @@ Mission::cruising_speed_sp_update() } void -Mission::do_abort_landing() +MissionBase::do_abort_landing() { // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT - if(_mission_type == MissionType::MISSION_TYPE_NONE) - { + if (_mission_type == MissionType::MISSION_TYPE_NONE) { return; } @@ -637,93 +619,7 @@ Mission::do_abort_landing() _navigator->publish_vehicle_cmd(&vcmd); } -void -Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) -{ - /* inform about the change */ - _navigator->get_mission_result()->item_do_jump_changed = true; - _navigator->get_mission_result()->item_changed_index = index; - _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; - - _navigator->set_mission_result_updated(); -} - -void -Mission::set_mission_item_reached() -{ - _navigator->get_mission_result()->seq_reached = _mission.current_seq; - _navigator->set_mission_result_updated(); - - reset_mission_item_reached(); -} - -void -Mission::set_mission_result() -{ - _navigator->get_mission_result()->finished = false; - _navigator->get_mission_result()->seq_current = _mission.current_seq; - - _navigator->set_mission_result_updated(); -} - -void -Mission::check_mission_valid() -{ - - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); - - _is_mission_valid = - _missionFeasibilityChecker.checkMissionFeasible(_mission, - _param_mis_dist_1wp.get(), - _param_mis_dist_wps.get()); - - _navigator->get_mission_result()->valid = _is_mission_valid; - _navigator->get_mission_result()->seq_total = _mission.count; - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); -} - -void -Mission::checkMissionRestart() -{ - if (_system_disarmed_while_inactive && _mission_has_been_activated) { - if (goToItem(0u, true) == EXIT_SUCCESS) { - _is_current_planned_mission_item_valid = true; - resetMissionJumpCounter(); - _navigator->reset_cruising_speed(); - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t"); - events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission"); - resetMission(); - } - } -} - -bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const -{ - return ((p1->valid == p2->valid) && - (p1->type == p2->type) && - (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && - (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && - (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (fabs(p1->lat - p2->lat) < DBL_EPSILON) && - (fabs(p1->lon - p2->lon) < DBL_EPSILON) && - (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && - ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && - (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && - (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && - (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && - (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && - (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && - ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) - && !PX4_ISFINITE(p2->cruising_throttle)))); - -} - -void Mission::publish_navigator_mission_item() +void MissionBase::publish_navigator_mission_item() { navigator_mission_item_s navigator_mission_item{}; @@ -753,59 +649,136 @@ void Mission::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } -void Mission::setCameraTrigger(bool enable) +Mission::Mission(Navigator *navigator) : + MissionBase(navigator) { - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // Pause trigger - cmd.param1 = -1.0f; - if(enable) - { - cmd.param3 = 0.0f; - } - else - { - cmd.param3 = 1.0f; - } - _navigator->publish_vehicle_cmd(&cmd); } -void Mission::setEndOfMissionSetpoint() +bool +Mission::set_current_mission_index(uint16_t index) { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - if (_land_detected_sub.get().landed) { - _mission_item.nav_cmd = NAV_CMD_IDLE; - - } else { - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + if (_is_mission_valid && (index < _mission.count)) { + if (goToItem(index, true) != EXIT_SUCCESS) { + // Keep the old mission index (it was not updated by the interface) and report back. + return false; } else { - setLoiterItemFromCurrentPosition(&_mission_item); + _is_current_planned_mission_item_valid = true; } + // update mission items if already in active mission + if (isActive()) { + // prevent following "previous - current" line + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + } + + return true; } - /* update position setpoint triplet */ - pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); - - // set mission finished - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); + return false; } -void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &num_found_items) +bool Mission::setNextMissionItem() { + return (goToNextItem(true) == EXIT_SUCCESS); +} + +bool +Mission::do_need_vertical_takeoff() +{ + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); + + if (_land_detected_sub.get().landed) { + /* force takeoff if landed (additional protection) */ + _need_takeoff = true; + + } else if (_global_pos_sub.get().alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { + /* if in-air and already above takeoff height, don't do takeoff */ + _need_takeoff = false; + + } else if (_global_pos_sub.get().alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() + && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { + /* if in-air but below takeoff height and we have a takeoff item */ + _need_takeoff = true; + } + + /* check if current mission item is one that requires takeoff before */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + + _need_takeoff = false; + return true; + } + } + + return false; +} + +bool +Mission::do_need_move_to_land() +{ + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { + + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + } + + return false; +} + +bool +Mission::do_need_move_to_takeoff() +{ + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + } + + return false; +} + +float +Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) +{ + /* calculate takeoff altitude */ + float takeoff_alt = get_absolute_altitude_for_item(*mission_item); + + /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + if (_land_detected_sub.get().landed) { + takeoff_alt = fmaxf(takeoff_alt, _global_pos_sub.get().alt + _navigator->get_takeoff_min_alt()); + + } else { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); + } + + return takeoff_alt; +} + +void Mission::setActiveMissionItems() +{ + /* Get mission item that comes after current if available */ + constexpr static size_t max_num_next_items{2u}; + mission_item_s next_mission_items[max_num_next_items]; + size_t num_found_items; + + getNextPositionItems(_mission.current_seq + 1, next_mission_items, num_found_items, max_num_next_items); + /*********************************** handle mission item *********************************************/ WorkItemType new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; @@ -818,9 +791,9 @@ void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &nu _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } - new_work_item_type = handleTakeoff(next_mission_items, num_found_items); + handleTakeoff(new_work_item_type, next_mission_items, num_found_items); - new_work_item_type = handleLanding(next_mission_items, num_found_items); + handleLanding(new_work_item_type, next_mission_items, num_found_items); // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { @@ -830,7 +803,7 @@ void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &nu // Only set the previous position item if the current one really changed if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && - !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { pos_sp_triplet->previous = current_setpoint_copy; } @@ -857,6 +830,7 @@ void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &nu /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } + } else if (item_contains_gate(_mission_item)) { // The mission item is a gate, let's check if the next item in the list provides // a position to go towards. @@ -877,11 +851,11 @@ void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &nu } } else { - new_work_item_type = handleVtolTransition(next_mission_items, num_found_items); - - issue_command(_mission_item); + handleVtolTransition(new_work_item_type, next_mission_items, num_found_items); } + issue_command(_mission_item); + /* set current work item type */ _work_item_type = new_work_item_type; @@ -903,15 +877,15 @@ void Mission::setMissionSetpoint(mission_item_s next_mission_items[], size_t &nu _navigator->set_position_setpoint_triplet_updated(); } -Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[], size_t &num_found_items) +void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* do takeoff before going to setpoint if needed and not already in takeoff */ /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ if (do_need_vertical_takeoff() && - _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TAKEOFF; @@ -924,9 +898,9 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] float takeoff_alt = calculate_takeoff_altitude(&_mission_item); mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home\t", - (double)(takeoff_alt - _navigator->get_home_position()->alt)); + (double)(takeoff_alt - _navigator->get_home_position()->alt)); events::send(events::ID("mission_takeoff_to"), events::Log::Info, - "Takeoff to {1:.1m_v} above home", takeoff_alt - _navigator->get_home_position()->alt); + "Takeoff to {1:.1m_v} above home", takeoff_alt - _navigator->get_home_position()->alt); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _global_pos_sub.get().lat; @@ -939,8 +913,8 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] _mission_item.time_inside = 0.0f; } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -948,7 +922,7 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] _mission_item.yaw = NAN; } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { // if the vehicle is already in fixed wing mode then the current mission item // will be accepted immediately and the work items will be skipped new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TAKEOFF; @@ -960,7 +934,7 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] /* if we just did a normal takeoff navigate to the actual waypoint now */ if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - _work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF) { + _work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF) { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ @@ -969,17 +943,17 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] /* if we just did a VTOL takeoff, prepare transition */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF && - _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_land_detected_sub.get().landed) { + _work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { /* disable weathervane before front transition for allowing yaw to align */ pos_sp_triplet->current.disable_weather_vane = true; /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ _mission_item.yaw = get_bearing_to_next_waypoint( - _global_pos_sub.get().lat, _global_pos_sub.get().lon, - _mission_item.lat, _mission_item.lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); _mission_item.force_heading = true; @@ -992,16 +966,16 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] /* heading is aligned now, prepare transition */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN && - _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_land_detected_sub.get().landed) { + _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { /* re-enable weather vane again after alignment */ pos_sp_triplet->current.disable_weather_vane = false; /* check if the vtol_takeoff waypoint is on top of us */ if (do_need_move_to_takeoff()) { - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF; + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON; } set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); @@ -1013,26 +987,25 @@ Mission::WorkItemType Mission::handleTakeoff(mission_item_s next_mission_items[] /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) { + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITON) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; } - - return new_work_item_type; } -Mission::WorkItemType Mission::handleLanding(mission_item_s next_mission_items[], size_t &num_found_items) +void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* move to land wp as fixed wing */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) - && !_land_detected_sub.get().landed) { + && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITON) + && !_land_detected_sub.get().landed) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; @@ -1056,9 +1029,9 @@ Mission::WorkItemType Mission::handleLanding(mission_item_s next_mission_items[] /* transition to MC */ if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_land_detected_sub.get().landed) { + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_land_detected_sub.get().landed) { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); _mission_item.altitude = _global_pos_sub.get().alt; @@ -1073,8 +1046,8 @@ Mission::WorkItemType Mission::handleLanding(mission_item_s next_mission_items[] /* move to landing waypoint before descent if necessary */ if (do_need_move_to_land() && - (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; @@ -1092,7 +1065,7 @@ Mission::WorkItemType Mission::handleLanding(mission_item_s next_mission_items[] float altitude = _global_pos_sub.get().alt; if (pos_sp_triplet->current.valid - && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { altitude = pos_sp_triplet->current.alt; } @@ -1144,22 +1117,20 @@ Mission::WorkItemType Mission::handleLanding(mission_item_s next_mission_items[] if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { _mission_item.yaw = NAN; } - - return new_work_item_type; } -Mission::WorkItemType Mission::handleVtolTransition(mission_item_s next_mission_items[], size_t &num_found_items) +void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* turn towards next waypoint before MC to FW transition */ if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT - && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_land_detected_sub.get().landed - && (num_found_items > 0u)) { + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_land_detected_sub.get().landed + && (num_found_items > 0u)) { /* disable weathervane before front transition for allowing yaw to align */ pos_sp_triplet->current.disable_weather_vane = true; @@ -1175,7 +1146,7 @@ Mission::WorkItemType Mission::handleVtolTransition(mission_item_s next_mission_ /* yaw is aligned now */ if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN && - new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; @@ -1186,6 +1157,4 @@ Mission::WorkItemType Mission::handleVtolTransition(mission_item_s next_mission_ // keep current setpoints (FW position controller generates wp to track during transition) pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; } - - return new_work_item_type; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 2e7540acea..2705042b2d 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -71,30 +71,25 @@ class Navigator; -class Mission : public MissionBlock, public ModuleParams, protected PlannedMissionInterface +class MissionBase : public MissionBlock, public ModuleParams, protected PlannedMissionInterface { public: - Mission(Navigator *navigator); - ~Mission() override = default; + MissionBase(Navigator *navigator); + ~MissionBase() override = default; - void on_inactive() override; - void on_inactivation() override; - void on_activation() override; - void on_active() override; + virtual void on_inactive() override; + virtual void on_inactivation() override; + virtual void on_activation() override; + virtual void on_active() override; - bool set_current_mission_index(uint16_t index); - - uint16_t get_land_start_index() const { return _land_start_index; } - bool get_land_start_available() const { return _land_start_index != _invalid_index; } - -private: +protected: // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message enum class WorkItemType { WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ - WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, + WORK_ITEM_TYPE_TRANSITON, WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, WORK_ITEM_TYPE_PRECISION_LAND } _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ @@ -104,12 +99,10 @@ private: MISSION_TYPE_MISSION } _mission_type{MissionType::MISSION_TYPE_NONE}; - void onMissionUpdate(bool has_mission_items_changed) override; - /** * Update mission topic */ - void update_mission(); + virtual void update_mission(); /** * Move on to next mission item or switch to loiter @@ -125,34 +118,51 @@ private: void set_mission_items(); /** - * Returns true if we need to do a takeoff at the current state + * Set the mission result */ - bool do_need_vertical_takeoff(); + void set_mission_result(); + + virtual void setActiveMissionItems() = 0; + virtual bool setNextMissionItem() = 0; + void setEndOfMissionItems(); + void publish_navigator_mission_item(); + bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + + hrt_abstime _time_mission_deactivated{0}; + + bool _is_current_planned_mission_item_valid{false}; + bool _is_mission_valid{false}; + bool _mission_has_been_activated{false}; + bool _initialized_mission_checked{false}; + bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _system_disarmed_while_inactive{false}; + + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; +private: + /** + * On mission update + * Change behaviour after external mission update. + * @param[in] has_mission_items_changed flag if the mission items have been changed. + */ + void onMissionUpdate(bool has_mission_items_changed) override; /** - * Returns true if we need to move to waypoint location before starting descent + * Check whether a mission is ready to go */ - bool do_need_move_to_land(); + void check_mission_valid(); /** - * Returns true if we need to move to waypoint location after vtol takeoff + * Reset mission */ - bool do_need_move_to_takeoff(); + void checkMissionRestart(); /** - * Copies position from setpoint if valid, otherwise copies current position + * Set a mission item as reached */ - void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); - - /** - * Create mission item to align towards next waypoint - */ - void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); - - /** - * Calculate takeoff height for mission item considering ground clearance - */ - float calculate_takeoff_altitude(struct mission_item_s *mission_item); + void set_mission_item_reached(); /** * Updates the heading of the vehicle. Rotary wings only. @@ -175,63 +185,59 @@ private: void report_do_jump_mission_changed(int index, int do_jumps_remaining); /** - * Set a mission item as reached + * Set the Camera Trigger + * Enable or disable the camera trigger for a mission. + * @param enable flag if the camera trigger should be enabled. */ - void set_mission_item_reached(); - - /** - * Set the mission result - */ - void set_mission_result(); - - /** - * Check whether a mission is ready to go - */ - void check_mission_valid(); - - /** - * Reset mission - */ - void checkMissionRestart(); - - bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; - - void publish_navigator_mission_item(); - void setCameraTrigger(bool enable); - void setEndOfMissionSetpoint(); - - void setMissionSetpoint(mission_item_s next_mission_items[], size_t &num_found_items); - - WorkItemType handleTakeoff(mission_item_s next_mission_items[], size_t &num_found_items); - - WorkItemType handleLanding(mission_item_s next_mission_items[], size_t &num_found_items); - - WorkItemType handleVtolTransition(mission_item_s next_mission_items[], size_t &num_found_items); - - DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamFloat) _param_mis_dist_wps, (ParamInt) _param_mis_mnt_yaw_ctl ) - - uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; - - uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ - - bool _is_current_planned_mission_item_valid{false}; - - bool _initialized_mission_checked{false}; - bool _is_mission_valid{false}; - - bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - - hrt_abstime _time_mission_deactivated{0}; - - bool _mission_has_been_activated{false}; - bool _system_disarmed_while_inactive{false}; +}; + +class Mission : public MissionBase +{ +public: + Mission(Navigator *navigator); + ~Mission() = default; + + bool set_current_mission_index(uint16_t index); + + uint16_t get_land_start_index() const { return _land_start_index; } + bool get_land_start_available() const { return _land_start_index != _invalid_index; } + +private: + bool setNextMissionItem() override; + + /** + * Returns true if we need to do a takeoff at the current state + */ + bool do_need_vertical_takeoff(); + + /** + * Returns true if we need to move to waypoint location before starting descent + */ + bool do_need_move_to_land(); + + /** + * Returns true if we need to move to waypoint location after vtol takeoff + */ + bool do_need_move_to_takeoff(); + + /** + * Calculate takeoff height for mission item considering ground clearance + */ + float calculate_takeoff_altitude(struct mission_item_s *mission_item); + + void setActiveMissionItems() override; + + void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); + + void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); + + void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items); }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2b6482c698..fa3f578fcd 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -938,6 +938,37 @@ float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission } } +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() { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 2a1c8f199b..04059d2a1f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -127,6 +127,18 @@ public: _payload_deploy_timeout_s = timeout_s; } + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const; + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const; + protected: /** * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index abf6147927..6be68f0980 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -277,6 +277,7 @@ public: int get_mission_landing_index() { return _mission.get_land_start_index(); } // RTL + bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } bool abort_landing(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 223311dcf7..79581ce25f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -549,7 +549,8 @@ void Navigator::run() * use MAV_CMD_MISSION_START to start the mission there */ uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; - if (_mission.get_land_start_available()) { + + if (_mission.get_land_start_available()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 028ddc3be1..b482601787 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,133 +42,206 @@ #include "rtl.h" #include "navigator.h" -#include + +#include #include -#include - - -static constexpr float DELAY_SIGMA = 0.01f; - using namespace time_literals; using namespace math; RTL::RTL(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + NavigatorMode(navigator), + ModuleParams(navigator), + _rtl_direct(navigator), + _rtl_mission(navigator), + _rtl_mission_reverse(navigator) { - _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); - _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); - _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); - _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); - _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); + initMission(); } void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_inactivation(); + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_inactivation(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactivation(); + break; + + default: + break; } } void RTL::on_inactive() { - // Limit inactive calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); + PlannedMissionInterface::update(); + _global_pos_sub.update(); + _vehicle_status_sub.update(); - if (_navigator->home_global_position_valid()) { - find_RTL_destination(); + _rtl_mission.on_inactive(); + _rtl_mission_reverse.on_inactive(); + _rtl_direct.on_inactive(); + + // Limit inactive calculation to 1Hz + hrt_abstime now{hrt_absolute_time()}; + + if ((now - _destination_check_time) > 1_s) { + _destination_check_time = now; + setRtlTypeAndDestination(); + + rtl_time_estimate_s estimated_time; + + switch (_rtl_type) { + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_MISSION_FAST: + estimated_time = _rtl_mission.calc_rtl_time_estimate(); + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate(); + break; + + default: + break; } - calc_and_pub_rtl_time_estimate(RTLState::RTL_STATE_NONE); + _rtl_time_estimate_pub.publish(estimated_time); } } -void RTL::find_RTL_destination() +void RTL::on_activation() { - // get home position: - home_position_s &home_landing_position = *_navigator->get_home_position(); + setRtlTypeAndDestination(); - // get global position - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_activation(); + break; - // set destination to home per default, then check if other valid landing spot is closer - _destination.set(home_landing_position); + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_activation(); + break; - // get distance to home position - double dlat = home_landing_position.lat - global_position.lat; - double dlon = home_landing_position.lon - global_position.lon; + case RtlType::RTL_DIRECT: + _rtl_direct.on_activation(_enforce_rtl_alt); + break; - double lon_scale = cos(radians(global_position.lat)); + default: + break; + } +} - auto coord_dist_sq = [lon_scale](double lat_diff, double lon_diff) -> double { - double lon_diff_scaled = lon_scale * matrix::wrap(lon_diff, -180., 180.); - return lat_diff * lat_diff + lon_diff_scaled * lon_diff_scaled; - }; +void RTL::on_active() +{ + PlannedMissionInterface::update(); + _global_pos_sub.update(); + _vehicle_status_sub.update(); - double min_dist_squared = coord_dist_sq(dlat, dlon); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + _rtl_mission.on_active(); + _rtl_mission_reverse.on_inactive(); + _rtl_direct.on_inactive(); + break; - _destination.type = RTL_DESTINATION_HOME; + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_reverse.on_active(); + _rtl_mission.on_inactive(); + _rtl_direct.on_inactive(); + break; - const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + case RtlType::RTL_DIRECT: + _rtl_direct.on_active(); + _rtl_mission_reverse.on_inactive(); + _rtl_mission.on_inactive(); + break; + default: + break; + } +} - // consider the mission landing if not RTL_TYPE_HOME_OR_RALLY type set - if (_param_rtl_type.get() != RTL_TYPE_HOME_OR_RALLY && _navigator->get_mission_start_land_available()) { - double mission_landing_lat; - double mission_landing_lon; - float mission_landing_alt; - - if (vtol_in_rw_mode) { - mission_landing_lat = _navigator->get_mission_landing_lat(); - mission_landing_lon = _navigator->get_mission_landing_lon(); - mission_landing_alt = _navigator->get_mission_landing_alt(); +void RTL::setRtlTypeAndDestination() +{ + if (_param_rtl_type.get() == 2) { + if (hasMissionLandStart()) { + int32_t mission_start_index{getClosestMissionItemIndex(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get())}; + _rtl_mission.setInitialMissionIndex(mission_start_index); + _rtl_type = RtlType::RTL_MISSION_FAST; } else { - mission_landing_lat = _navigator->get_mission_landing_start_lat(); - mission_landing_lon = _navigator->get_mission_landing_start_lon(); - mission_landing_alt = _navigator->get_mission_landing_start_alt(); + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; } - dlat = mission_landing_lat - global_position.lat; - dlon = mission_landing_lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + } else { + // check the closest allowed destination. + bool isMissionLanding{false}; + RtlDirect::RtlPosition rtl_position; + float rtl_alt; + findRtlDestination(isMissionLanding, rtl_position, rtl_alt); - // always find closest destination if in hover and VTOL - if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) { - - // compare home position to landing position to decide which is closer - if (dist_squared < min_dist_squared) { - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; - } + if (isMissionLanding) { + _rtl_mission.setInitialMissionIndex(_land_start_index); + _rtl_type = RtlType::RTL_MISSION_FAST; } else { - // it has to be the mission landing - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + _rtl_type = RtlType::RTL_DIRECT; } } +} - // do not consider rally point if RTL type is set to RTL_TYPE_MISSION_LANDING_REVERSED, so exit function and use either home or mission landing - if (_param_rtl_type.get() == RTL_TYPE_MISSION_LANDING_REVERSED) { - return; +void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +{ + // set destination to home per default, then check if other valid landing spot is closer + rtl_position.alt = _home_pos_sub.get().alt; + rtl_position.lat = _home_pos_sub.get().lat; + rtl_position.lon = _home_pos_sub.get().lon; + rtl_position.yaw = _home_pos_sub.get().yaw; + isMissionLanding = false; + + // get distance to home position + float min_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; + + const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + // consider the mission landing if available and allowed + if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _land_pos.lat, _land_pos.lon)}; + + // always find closest destination if in hover and VTOL + if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) { + // Use the mission landing destination. + min_dist = dist; + setLandPosAsDestination(rtl_position); + isMissionLanding = true; + + } else if (dist < min_dist) { + min_dist = dist; + setLandPosAsDestination(rtl_position); + isMissionLanding = true; + } } // compare to safe landing positions - mission_safe_point_s closest_safe_point {}; mission_stats_entry_s stats; + + /* if (dm_lock(dm_item_t::DM_KEY_SAFE_POINTS) != 0) { + return; + } */ + int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); int num_safe_points = 0; @@ -176,9 +249,6 @@ void RTL::find_RTL_destination() num_safe_points = stats.num_items; } - // check if a safe point is closer than home or landing - int closest_index = 0; - for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { mission_safe_point_s mission_safe_point; @@ -188,510 +258,78 @@ void RTL::find_RTL_destination() continue; } - // TODO: take altitude into account for distance measurement - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; + if (dist < min_dist) { + min_dist = dist; + setSafepointAsDestination(rtl_position, mission_safe_point); + isMissionLanding = false; } } - if (closest_index > 0) { - _destination.type = RTL_DESTINATION_SAFE_POINT; - - // There is a safe point closer than home/mission landing - // TODO: handle all possible mission_safe_point.frame cases - switch (closest_safe_point.frame) { - case 0: // MAV_FRAME_GLOBAL - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt; - _destination.yaw = home_landing_position.yaw; - break; - - case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home - _destination.yaw = home_landing_position.yaw; - break; - - default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); - events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", - closest_safe_point.frame); - break; - } - } + // dm_unlock(dm_item_t::DM_KEY_SAFE_POINTS); if (_param_rtl_cone_half_angle_deg.get() > 0 - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); } else { - _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); + rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); } } -void RTL::on_activation() +void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position) { - _rtl_state = RTL_STATE_NONE; - - // if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode - // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission - _should_engange_mission_for_landing = (_destination.type == RTL_DESTINATION_MISSION_LANDING) - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - // output the correct message, depending on where the RTL destination is - switch (_destination.type) { - case RTL_DESTINATION_HOME: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.\t"); - events::send(events::ID("rtl_land_at_home"), events::Log::Info, "RTL: landing at home position"); - break; - - case RTL_DESTINATION_MISSION_LANDING: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.\t"); - events::send(events::ID("rtl_land_at_mission"), events::Log::Info, "RTL: landing at mission landing"); - break; - - case RTL_DESTINATION_SAFE_POINT: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.\t"); - events::send(events::ID("rtl_land_at_safe_point"), events::Log::Info, "RTL: landing at safe landing point"); - break; - } - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - if (_navigator->get_land_detected()->landed) { - // For safety reasons don't go into RTL if landed. - _rtl_state = RTL_STATE_LANDED; - - } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { - // we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode, - // which will continue executing the landing - _rtl_state = RTL_STATE_LAND; - - - } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { - - // If lower than return altitude, climb up first. - // If rtl_alt_min is true then forcing altitude change even if above. - _rtl_state = RTL_STATE_CLIMB; - - } else { - // Otherwise go straight to return - _rtl_state = RTL_STATE_RETURN; - } - - // reset cruising speed and throttle to default for RTL - _navigator->set_cruising_speed(); - _navigator->set_cruising_throttle(); - - set_rtl_item(); - + rtl_position.alt = _land_pos.alt; + rtl_position.lat = _land_pos.lat; + rtl_position.lon = _land_pos.lon; + rtl_position.yaw = _home_pos_sub.get().yaw; } -void RTL::on_active() +void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, + const mission_safe_point_s &mission_safe_point) { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { - advance_rtl(); - set_rtl_item(); - } - - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - // Limit rtl time calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); - calc_and_pub_rtl_time_estimate(_rtl_state); - } -} - -void RTL::set_rtl_item() -{ - _navigator->set_can_loiter_at_sp(false); - - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - - // if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission - const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ? - _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get(); - - const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - - _mission_item.lat = gpos.lat; - _mission_item.lon = gpos.lon; - _mission_item.altitude = _rtl_alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); - events::send(events::ID("rtl_climb_to"), events::Log::Info, - "RTL: climb to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); - break; - } - - case RTL_STATE_RETURN: { - - // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status - // can be displayed on groundstation and the WP is accepted once within loiter radius - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - - - } else { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _rtl_alt; // Don't change altitude - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && - destination_dist > _param_rtl_min_dist.get()) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || - destination_dist < _param_rtl_min_dist.get()) { - // Use destination yaw if close to _destination. - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_return_at"), events::Log::Info, - "RTL: return at {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // Except for vtol which might be still off here and should point towards this location. - const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_descend_to"), events::Log::Info, - "RTL: descend to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - break; - } - - case RTL_STATE_LOITER: { - const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); - - if (autocontinue) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", - (double)_param_rtl_land_delay.get()); - events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); - - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); - events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; // Don't change altitude. - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autocontinue; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - _navigator->set_can_loiter_at_sp(true); - - break; - } - - case RTL_STATE_HEAD_TO_CENTER: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - break; - } - - case RTL_STATE_TRANSITION_TO_MC: { - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - break; - } - - case RTL_MOVE_TO_LAND_HOVER_VTOL: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.origin = ORIGIN_ONBOARD; - break; - } - - case RTL_STATE_LAND: { - // Land at destination. - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _destination.alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.land_precision = _param_rtl_pld_md.get(); - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); - events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); - break; - } - - case RTL_STATE_LANDED: { - set_idle_item(&_mission_item); - set_return_alt_min(false); - break; - } - - default: - break; - } - - reset_mission_item_reached(); - - // Execute command if set. This is required for commands like VTOL transition. - if (!item_contains_position(_mission_item)) { - issue_command(_mission_item); - } - - // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); - - if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { - _navigator->set_position_setpoint_triplet_updated(); - } -} - -void RTL::advance_rtl() -{ - // determines if the vehicle should loiter above land - const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; - - // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (mission_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt; + rtl_position.yaw = _home_pos_sub.get().yaw;; break; - case RTL_STATE_RETURN: - if (vtol_in_fw_mode || descend_and_loiter) { - _rtl_state = RTL_STATE_DESCEND; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_DESCEND: - - if (descend_and_loiter) { - _rtl_state = RTL_STATE_LOITER; - - } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_LOITER: - - if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_TRANSITION_TO_MC; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - _rtl_state = RTL_STATE_LAND; - break; - - case RTL_STATE_HEAD_TO_CENTER: - - _rtl_state = RTL_STATE_TRANSITION_TO_MC; - - break; - - case RTL_STATE_TRANSITION_TO_MC: - - _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; - - break; - - case RTL_MOVE_TO_LAND_HOVER_VTOL: - - _rtl_state = RTL_STATE_LAND; - - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home + rtl_position.yaw = _home_pos_sub.get().yaw;; break; default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); + events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", + mission_safe_point.frame); break; } } -float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) +float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, + float cone_half_angle_deg) { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - // horizontal distance to destination - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + rtl_position.lat, rtl_position.lon); // minium rtl altitude to use when outside of horizontal acceptance radius of target position. // We choose the minimum height to be two times the distance from the land position in order to // avoid the vehicle touching the ground while still moving horizontally. - const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * - _navigator->get_acceptance_radius(); + const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); + float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); - if (destination_dist <= _navigator->get_acceptance_radius()) { - return_altitude_amsl = _destination.alt + 2.0f * destination_dist; + if (destination_dist <= _param_nav_acc_rad.get()) { + return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { @@ -701,7 +339,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); // minimum altitude we need in order to be within the user defined cone - const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt; + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); } @@ -709,227 +347,5 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, gpos.alt); -} - -void RTL::calc_and_pub_rtl_time_estimate(const RTLState rtl_state) -{ - rtl_time_estimate_s rtl_time_estimate{}; - - // Calculate RTL time estimate only when there is a valid home position - // TODO: Also check if vehicle position is valid - if (!_navigator->home_global_position_valid()) { - rtl_time_estimate.valid = false; - - } else { - rtl_time_estimate.valid = true; - - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - - // Sum up time estimate for various segments of the landing procedure - switch (rtl_state) { - case RTL_STATE_NONE: - case RTL_STATE_CLIMB: { - // Climb segment is only relevant if the drone is below return altitude - const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0; - - if (climb_dist > 0) { - rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); - } - } - - // FALLTHROUGH - case RTL_STATE_RETURN: - - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed(); - - // FALLTHROUGH - case RTL_STATE_HEAD_TO_CENTER: - case RTL_STATE_TRANSITION_TO_MC: - case RTL_STATE_DESCEND: { - // when descending, the target altitude is stored in the current mission item - float initial_altitude = 0; - float loiter_altitude = 0; - - if (rtl_state == RTL_STATE_DESCEND) { - // Take current vehicle altitude as the starting point for calculation - initial_altitude = gpos.alt; // TODO: Check if this is in the right frame - loiter_altitude = _mission_item.altitude; // Next waypoint = loiter - - - } else { - // Take the return altitude as the starting point for the calculation - initial_altitude = _rtl_alt; // CLIMB and RETURN - loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - } - - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); - } - - // FALLTHROUGH - case RTL_STATE_LOITER: - // Add land delay (the short pause for deploying landing gear) - // TODO: Check if landing gear is deployed or not - rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); - - // FALLTHROUGH - case RTL_MOVE_TO_LAND_HOVER_VTOL: - case RTL_STATE_LAND: { - float initial_altitude; - - // Add land segment (second landing phase) which comes after LOITER - if (rtl_state == RTL_STATE_LAND) { - // If we are in this phase, use the current vehicle altitude instead - // of the altitude paramteter to get a continous time estimate - initial_altitude = gpos.alt; - - - } else { - // If this phase is not active yet, simply use the loiter altitude, - // which is where the LAND phase will start - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - initial_altitude = loiter_altitude; - } - - // Prevent negative times when close to the ground - if (initial_altitude > _destination.alt) { - rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); - } - - } - - break; - - case RTL_STATE_LANDED: - // Remaining time is 0 - break; - } - - // Prevent negative durations as phyiscally they make no sense. These can - // occur during the last phase of landing when close to the ground. - rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); - - // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin - rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate - + _param_rtl_time_margin.get(); - } - - // Publish message - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); -} - -float RTL::getCruiseSpeed() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getHoverLandSpeed() -{ - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - -matrix::Vector2f RTL::get_wind() -{ - _wind_sub.update(); - matrix::Vector2f wind; - - if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { - wind(0) = _wind_sub.get().windspeed_north; - wind(1) = _wind_sub.get().windspeed_east; - } - - return wind; -} - -float RTL::getClimbRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - - if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getDescendRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getCruiseGroundSpeed() -{ - float cruise_speed = getCruiseSpeed(); - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - matrix::Vector2f wind = get_wind(); - - matrix::Vector2f to_destination_vec; - get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, - &to_destination_vec(0), &to_destination_vec(1)); - - const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); - - const float wind_towards_home = wind.dot(to_home_dir); - const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); - - - // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient - const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( - 0.f, wind_towards_home); - - cruise_speed = ground_speed; - } - - return cruise_speed; + return max(return_altitude_amsl, _global_pos_sub.get().alt); } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 7b9320a87a..47425035b9 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -41,58 +41,32 @@ #pragma once +#include #include #include "navigator_mode.h" -#include "mission_block.h" +#include "lib/mission/planned_mission_interface.h" +#include "rtl_direct.h" +#include "rtl_mission_fast.h" +#include "rtl_mission_fast_reverse.h" -#include -#include +#include +#include #include -#include -#include -#include -#include class Navigator; -class RTL : public MissionBlock, public ModuleParams +class RTL : public NavigatorMode, protected PlannedMissionInterface, public ModuleParams { public: RTL(Navigator *navigator); ~RTL() = default; - enum RTLType { - RTL_TYPE_HOME_OR_RALLY = 0, - RTL_TYPE_MISSION_LANDING, - RTL_TYPE_MISSION_LANDING_REVERSED, - RTL_TYPE_CLOSEST, - }; - - enum RTLDestinationType { - RTL_DESTINATION_HOME = 0, - RTL_DESTINATION_MISSION_LANDING, - RTL_DESTINATION_SAFE_POINT, - }; - - enum RTLHeadingMode { - RTL_NAVIGATION_HEADING = 0, - RTL_DESTINATION_HEADING, - RTL_CURRENT_HEADING, - }; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - RTL_STATE_HEAD_TO_CENTER, + enum class RtlType { + RTL_DIRECT, + RTL_MISSION_FAST, + RTL_MISSION_FAST_REVERSE, }; void on_inactivation() override; @@ -100,97 +74,64 @@ public: void on_activation() override; void on_active() override; - void find_RTL_destination(); + void initialize() override {}; - void set_return_alt_min(bool min) { _rtl_alt_min = min; } - - int get_rtl_type() const { return _param_rtl_type.get(); } - - void get_rtl_xy_z_speed(float &xy, float &z); - - matrix::Vector2f get_wind(); - - RTLState getRTLState() { return _rtl_state; } - - bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; } + void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } private: + void onMissionUpdate(bool has_mission_items_changed) override {}; - void set_rtl_item(); + void setRtlTypeAndDestination(); - void advance_rtl(); + /** + * @brief Find RTL destination. + * + */ + void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); - float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); - void calc_and_pub_rtl_time_estimate(const RTLState rtl_state); + /** + * @brief Set the position of the land start marker in the planned mission as destination. + * + */ + void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position); - float getCruiseGroundSpeed(); + /** + * @brief Set the safepoint as destination. + * + * @param mission_safe_point is the mission safe point/rally point to set as destination. + */ + void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point); - float getClimbRate(); - - float getDescendRate(); - - float getCruiseSpeed(); - - float getHoverLandSpeed(); - - RTLState _rtl_state{RTL_STATE_NONE}; - - struct RTLPosition { - double lat; - double lon; - float alt; - float yaw; - uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) - RTLDestinationType type{RTL_DESTINATION_HOME}; - - void set(const home_position_s &home_position) - { - lat = home_position.lat; - lon = home_position.lon; - alt = home_position.alt; - yaw = home_position.yaw; - safe_point_index = 0; - type = RTL_DESTINATION_HOME; - } - }; - - RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) + /** + * @brief + * + * @param cone_half_angle_deg + * @return float + */ + float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); hrt_abstime _destination_check_time{0}; - float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position + RtlType _rtl_type{RtlType::RTL_DIRECT}; - bool _rtl_alt_min{false}; - bool _should_engange_mission_for_landing{false}; + RtlDirect _rtl_direct; + + RtlMissionFast _rtl_mission; + + RtlMissionFastReverse _rtl_mission_reverse; + + bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, - (ParamFloat) _param_rtl_descend_alt, - (ParamFloat) _param_rtl_land_delay, - (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamInt) _param_rtl_pld_md, - (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md, - (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_min_dist, + (ParamFloat) _param_nav_acc_rad ) - param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; - param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; - param_t _param_mpc_land_speed{PARAM_INVALID}; - param_t _param_fw_climb_rate{PARAM_INVALID}; - param_t _param_fw_sink_rate{PARAM_INVALID}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - param_t _param_fw_airspeed_trim{PARAM_INVALID}; - param_t _param_mpc_xy_cruise{PARAM_INVALID}; - param_t _param_rover_cruise_speed{PARAM_INVALID}; - - uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; - -float time_to_home(const matrix::Vector3f &to_home_vec, - const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, - float vehicle_descent_speed_m_s); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp new file mode 100644 index 0000000000..8f8de6a467 --- /dev/null +++ b/src/modules/navigator/rtl_direct.cpp @@ -0,0 +1,714 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 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 rtl.cpp + * + * Helper class to access RTL + * + * @author Julian Oes + * @author Anton Babushkin + * @author Julian Kent + */ + +#include "rtl_direct.h" +#include "navigator.h" +#include +#include + +#include + + +static constexpr float DELAY_SIGMA = 0.01f; + +using namespace time_literals; +using namespace math; + +RtlDirect::RtlDirect(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); + _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); + _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); + _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); + _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); + _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); +} + +void RtlDirect::on_activation(bool enforce_rtl_alt) +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _wind_sub.update(); + + if (_land_detected_sub.get().landed) { + // For safety reasons don't go into RTL if landed. + _rtl_state = RTL_STATE_LANDED; + + } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If rtl_alt_min is true then forcing altitude change even if above. + _rtl_state = RTL_STATE_CLIMB; + + } else { + // Otherwise go start with climb + _rtl_state = RTL_STATE_RETURN; + } + + // reset cruising speed and throttle to default for RTL + _navigator->set_cruising_speed(); + _navigator->set_cruising_throttle(); + + set_rtl_item(); + + MissionBlock::on_active(); +} + +void RtlDirect::on_active() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _wind_sub.update(); + + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { + advance_rtl(); + set_rtl_item(); + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + // Need to update the position and type on the current setpoint triplet. + _navigator->get_precland()->on_active(); + } +} + +void RtlDirect::set_rtl_item() +{ + _navigator->set_can_loiter_at_sp(false); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, + // even if current climb altitude is below (e.g. RTL immediately after take off) + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("rtl_climb_to"), events::Log::Info, + "RTL: climb to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + break; + } + + case RTL_STATE_RETURN: { + + // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status + // can be displayed on groundstation and the WP is accepted once within loiter radius + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + + + } else { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _rtl_alt; // Don't change altitude + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && + destination_dist > _param_rtl_min_dist.get()) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || + destination_dist < _param_rtl_min_dist.get()) { + // Use destination yaw if close to _destination. + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_return_at"), events::Log::Info, + "RTL: return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // Except for vtol which might be still off here and should point towards this location. + const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_descend_to"), events::Log::Info, + "RTL: descend to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + break; + } + + case RTL_STATE_LOITER: { + const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); + + if (autocontinue) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", + (double)_param_rtl_land_delay.get()); + events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); + events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; // Don't change altitude. + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autocontinue; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + _navigator->set_can_loiter_at_sp(true); + + break; + } + + case RTL_STATE_HEAD_TO_CENTER: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + break; + } + + case RTL_STATE_TRANSITION_TO_MC: { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + break; + } + + case RTL_MOVE_TO_LAND_HOVER_VTOL: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.origin = ORIGIN_ONBOARD; + break; + } + + case RTL_STATE_LAND: { + // Land at destination. + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _destination.alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision > 0u && _mission_item.land_precision <= 2u) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + } + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); + events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); + break; + } + + case RTL_STATE_LANDED: { + set_idle_item(&_mission_item); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + // Execute command if set. This is required for commands like VTOL transition. + if (!MissionBlock::item_contains_position(_mission_item)) { + issue_command(_mission_item); + } + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void RtlDirect::advance_rtl() +{ + // determines if the vehicle should loiter above land + const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + + // vehicle is a vtol and currently in fixed wing mode + const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + if (vtol_in_fw_mode || descend_and_loiter) { + _rtl_state = RTL_STATE_DESCEND; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_DESCEND: + + if (descend_and_loiter) { + _rtl_state = RTL_STATE_LOITER; + + } else if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_LOITER: + + if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_HEAD_TO_CENTER: + + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + break; + + case RTL_STATE_TRANSITION_TO_MC: + + _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; + + break; + + case RTL_MOVE_TO_LAND_HOVER_VTOL: + + _rtl_state = RTL_STATE_LAND; + + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} + +rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() +{ + rtl_time_estimate_s rtl_time_estimate{}; + + RTLState start_state_for_estimate{RTL_STATE_NONE}; + + if (isActive()) { + start_state_for_estimate = _rtl_state; + } + + // Calculate RTL time estimate only when there is a valid home position + // TODO: Also check if vehicle position is valid + if (!_navigator->home_global_position_valid()) { + rtl_time_estimate.valid = false; + + } else { + rtl_time_estimate.valid = true; + + // Sum up time estimate for various segments of the landing procedure + switch (start_state_for_estimate) { + case RTL_STATE_NONE: + case RTL_STATE_CLIMB: { + // Climb segment is only relevant if the drone is below return altitude + const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0; + + if (climb_dist > 0) { + rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); + } + } + + // FALLTHROUGH + case RTL_STATE_RETURN: + + // Add cruise segment to home + rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + + // FALLTHROUGH + case RTL_STATE_HEAD_TO_CENTER: + case RTL_STATE_TRANSITION_TO_MC: + case RTL_STATE_DESCEND: { + // when descending, the target altitude is stored in the current mission item + float initial_altitude = 0; + float loiter_altitude = 0; + + if (start_state_for_estimate == RTL_STATE_DESCEND) { + // Take current vehicle altitude as the starting point for calculation + initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame + loiter_altitude = _mission_item.altitude; // Next waypoint = loiter + + + } else { + // Take the return altitude as the starting point for the calculation + initial_altitude = _rtl_alt; // CLIMB and RETURN + loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + } + + // Add descend segment (first landing phase: return alt to loiter alt) + rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + } + + // FALLTHROUGH + case RTL_STATE_LOITER: + // Add land delay (the short pause for deploying landing gear) + // TODO: Check if landing gear is deployed or not + rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); + + // FALLTHROUGH + case RTL_MOVE_TO_LAND_HOVER_VTOL: + case RTL_STATE_LAND: { + float initial_altitude; + + // Add land segment (second landing phase) which comes after LOITER + if (start_state_for_estimate == RTL_STATE_LAND) { + // If we are in this phase, use the current vehicle altitude instead + // of the altitude paramteter to get a continous time estimate + initial_altitude = _global_pos_sub.get().alt; + + + } else { + // If this phase is not active yet, simply use the loiter altitude, + // which is where the LAND phase will start + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + initial_altitude = loiter_altitude; + } + + // Prevent negative times when close to the ground + if (initial_altitude > _destination.alt) { + rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); + } + + } + + break; + + case RTL_STATE_LANDED: + // Remaining time is 0 + break; + } + + // Prevent negative durations as phyiscally they make no sense. These can + // occur during the last phase of landing when close to the ground. + rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); + + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate + + _param_rtl_time_margin.get(); + } + + // return message + rtl_time_estimate.timestamp = hrt_absolute_time(); + + return rtl_time_estimate; +} + +float RtlDirect::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getHoverLandSpeed() +{ + float ret = 1e6f; + + if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + + return ret; +} + +matrix::Vector2f RtlDirect::get_wind() +{ + _wind_sub.update(); + matrix::Vector2f wind; + + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; + } + + return wind; +} + +float RtlDirect::getClimbRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getDescendRate() +{ + float ret = 1e6f; + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getCruiseGroundSpeed() +{ + float cruise_speed = getCruiseSpeed(); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + matrix::Vector2f wind = get_wind(); + + matrix::Vector2f to_destination_vec; + get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon, + &to_destination_vec(0), &to_destination_vec(1)); + + const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); + + const float wind_towards_home = wind.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); + + + // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( + 0.f, wind_towards_home); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h new file mode 100644 index 0000000000..d698e3252a --- /dev/null +++ b/src/modules/navigator/rtl_direct.h @@ -0,0 +1,224 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_direct.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include + +#include "mission_block.h" +#include "lib/mission/planned_mission_interface.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Navigator; + +class RtlDirect : public MissionBlock, public ModuleParams +{ +public: + /** + * @brief Return to launch position. + * Defines the position and landing yaw for the return to launch destination. + * + */ + struct RtlPosition { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ + }; + + RtlDirect(Navigator *navigator); + + ~RtlDirect() = default; + + /** + * @brief On activation. + * Initialize the return to launch calculations. + * + * @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above. + */ + void on_activation(bool enforce_rtl_alt); + + /** + * @brief on active + * Update the return to launch calculation and set new setpoints for controller if necessary. + * + */ + void on_active() override; + + /** + * @brief Calculate the estimated time needed to return to launch. + * + * @return estimated time to return to launch. + */ + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setRtlAlt(float alt) {_rtl_alt = alt;}; + + void setRtlPosition(RtlPosition position) {_destination = position;}; + +private: + /** + * @brief Return to launch heading mode. + * + */ + enum RTLHeadingMode { + RTL_NAVIGATION_HEADING = 0, + RTL_DESTINATION_HEADING, + RTL_CURRENT_HEADING, + }; + + /** + * @brief Return to launch state machine. + * + */ + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + }; + +private: + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + /** + * @brief Set the return to launch control setpoint. + * + */ + void set_rtl_item(); + + /** + * @brief Advance the return to launch state machine. + * + */ + void advance_rtl(); + + /** + * @brief Get the Cruise Ground Speed + * + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(); + + /** + * @brief Get the climb rate + * + * @return Climb rate [m/s] + */ + float getClimbRate(); + + /** + * @brief Get the descend rate + * + * @return descend rate [m/s] + */ + float getDescendRate(); + + /** + * @brief Get the cruise speed + * + * @return cruise speed [m/s] + */ + float getCruiseSpeed(); + + /** + * @brief Get the Hover Land Speed + * + * @return Hover land speed [m/s] + */ + float getHoverLandSpeed(); + + /** Current state in the state machine.*/ + RTLState _rtl_state{RTL_STATE_NONE}; + + RtlPosition _destination{}; ///< the RTL position to fly to + + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + + DEFINE_PARAMETERS( + (ParamInt) _param_rtl_type, + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad, + (ParamInt) _param_rtl_hdg_md, + (ParamFloat) _param_rtl_time_factor, + (ParamInt) _param_rtl_time_margin, + (ParamFloat) _param_nav_acc_rad /**< acceptance for takeoff */ + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; + param_t _param_mpc_land_speed{PARAM_INVALID}; + param_t _param_fw_climb_rate{PARAM_INVALID}; + param_t _param_fw_sink_rate{PARAM_INVALID}; + param_t _param_fw_airspeed_trim{PARAM_INVALID}; + param_t _param_mpc_xy_cruise{PARAM_INVALID}; + param_t _param_rover_cruise_speed{PARAM_INVALID}; + + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; +}; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp new file mode 100644 index 0000000000..16419dd777 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -0,0 +1,246 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast.h" +#include "navigator.h" + +#include + +RtlMissionFast::RtlMissionFast(Navigator *navigator) : + MissionBase(navigator) +{ + +} + +void RtlMissionFast::on_activation() +{ + goToItem(_init_mission_index, false); + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFast::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFast::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +void RtlMissionFast::setInitialMissionIndex(int32_t init_mission_index) +{ + // Map the input to feasible indexes. + if (init_mission_index < 0) { + _init_mission_index = 0; + + } else if (init_mission_index >= _mission.count) { + _init_mission_index = _mission.count - 1; + + } else { + _init_mission_index = init_mission_index; + } +} + +bool RtlMissionFast::setNextMissionItem() +{ + return (goToNextPositionItem(true) == EXIT_SUCCESS); +} + +void RtlMissionFast::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + mission_item_s next_mission_item; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item, num_found_items, 1u); + + if (num_found_items > 0) { + + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous.valid = false; + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } + } +} + +bool RtlMissionFast::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h new file mode 100644 index 0000000000..432cb4c2de --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.h @@ -0,0 +1,75 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mission.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFast : public MissionBase +{ +public: + RtlMissionFast(Navigator *navigator); + ~RtlMissionFast() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setInitialMissionIndex(int32_t init_mission_index); + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + int32_t _init_mission_index{0}; + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +}; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp new file mode 100644 index 0000000000..77d418c2e2 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -0,0 +1,263 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast_reverse.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast_reverse.h" +#include "navigator.h" + +#include + + +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : + MissionBase(navigator) +{ + +} + +void RtlMissionFastReverse::on_activation() +{ + setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, + _home_pos_sub.get().alt, _vehicle_status_sub.get()); + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidate the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFastReverse::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFastReverse::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFastReverse::setNextMissionItem() +{ + return (goToPreviousPositionItem(true) == EXIT_SUCCESS); +} + +void RtlMissionFastReverse::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // Transition to fixed wing if necessary. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + mission_item_s next_mission_item; + size_t num_found_items = 0; + getPreviousPositionItems(_mission.current_seq - 1, &next_mission_item, num_found_items, 1u); + + if (num_found_items > 0) { + + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) || (_mission_item.nav_cmd == NAV_CMD_TAKEOFF)); + bool vtol_in_fw = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + if (needs_to_land) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Go to Take off location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TAKEOFF; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (vtol_in_fw) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF) { + // Go to home location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + } else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.yaw = NAN; + + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } else { + _mission_item.altitude = _home_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + pos_sp_triplet->previous.valid = false; + } + } + } +} + +bool RtlMissionFastReverse::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h new file mode 100644 index 0000000000..512f07c4b5 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * Copyright (c) 2023 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 rtl_mission_fast_reverse.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mission.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFastReverse : public MissionBase +{ +public: + RtlMissionFastReverse(Navigator *navigator); + ~RtlMissionFastReverse() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate(); + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ +};