From 48acf98fd55b559b2d4758f9234dc4a516ca1b5e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 9 Jan 2023 15:19:03 +0100 Subject: [PATCH] Navigator: remove deprecated _can_loiter_at_sp and _need_takeoff Signed-off-by: Silvan Fuhrer --- src/modules/navigator/land.cpp | 2 - src/modules/navigator/loiter.cpp | 3 -- src/modules/navigator/mission.cpp | 62 ------------------------ src/modules/navigator/mission.h | 7 --- src/modules/navigator/navigator.h | 4 -- src/modules/navigator/navigator_main.cpp | 1 - src/modules/navigator/rtl.cpp | 4 -- src/modules/navigator/takeoff.cpp | 7 --- 8 files changed, 90 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index d280c942c6..08513bfa9f 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -62,8 +62,6 @@ Land::on_activation() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(false); - _navigator->set_position_setpoint_triplet_updated(); // reset cruising speed to default diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 3cb1a3fae3..66fadb10c4 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -82,7 +82,6 @@ Loiter::set_loiter_position() // Not setting loiter position if disarmed and landed, instead mark the current // setpoint as invalid and idle (both, just to be sure). - _navigator->set_can_loiter_at_sp(false); _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _navigator->set_position_setpoint_triplet_updated(); return; @@ -110,7 +109,6 @@ Loiter::set_loiter_position() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } @@ -136,7 +134,6 @@ Loiter::reposition() memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); // mark this as done diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9e673c5297..75c4ce2d41 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -168,11 +168,6 @@ Mission::on_inactive() _inited = true; } - /* require takeoff after non-loiter or landing */ - if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { - _need_takeoff = true; - } - /* reset so current mission item gets restarted if mission was paused */ _work_item_type = WORK_ITEM_TYPE_DEFAULT; @@ -323,11 +318,6 @@ Mission::on_active() set_mission_items(); } - } else { - /* if waypoint position reached allow loiter on the setpoint */ - if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { - _navigator->set_can_loiter_at_sp(true); - } } /* see if we need to update the current yaw heading */ @@ -801,9 +791,6 @@ Mission::set_mission_items() } else { events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); } - - /* use last setpoint for loiter */ - _navigator->set_can_loiter_at_sp(true); } user_feedback_done = true; @@ -832,9 +819,6 @@ Mission::set_mission_items() 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(); @@ -1274,14 +1258,6 @@ Mission::set_mission_items() /* set current work item type */ _work_item_type = new_work_item_type; - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - - _need_takeoff = true; - } - - _navigator->set_can_loiter_at_sp(false); reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_MISSION) { @@ -1330,44 +1306,6 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } -bool -Mission::do_need_vertical_takeoff() -{ - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - if (_navigator->get_land_detected()->landed) { - /* force takeoff if landed (additional protection) */ - _need_takeoff = true; - - } else if (_navigator->get_global_position()->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 (_navigator->get_global_position()->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() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6b31f7ad76..49154c73ef 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -133,11 +133,6 @@ private: */ void set_mission_items(); - /** - * 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 */ @@ -348,8 +343,6 @@ private: float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ - bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - enum { MISSION_TYPE_NONE, MISSION_TYPE_MISSION diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 41b33edab2..e17786cbfc 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -142,7 +142,6 @@ public: /** * Setters */ - void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } void set_mission_result_updated() { _mission_result_updated = true; } @@ -171,8 +170,6 @@ public: Geofence &get_geofence() { return _geofence; } - bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } - float get_loiter_radius() { return _param_nav_loiter_rad.get(); } /** @@ -358,7 +355,6 @@ private: hrt_abstime _last_geofence_check = 0; bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ - bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9d9ed8c035..32b99ea7b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -851,7 +851,6 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_STAB: default: navigation_mode_new = nullptr; - _can_loiter_at_sp = false; break; } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 517b0f9c39..4dbb30e77d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -416,8 +416,6 @@ void RTL::on_active() 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(); @@ -577,8 +575,6 @@ void RTL::set_rtl_item() _mission_item.origin = ORIGIN_ONBOARD; _mission_item.loiter_radius = landing_loiter_radius; - _navigator->set_can_loiter_at_sp(true); - break; } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index f998b9c1d9..e81f446d56 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -181,12 +181,5 @@ Takeoff::set_takeoff_position() memset(rep, 0, sizeof(*rep)); } - if (PX4_ISFINITE(pos_sp_triplet->current.lat) && PX4_ISFINITE(pos_sp_triplet->current.lon)) { - _navigator->set_can_loiter_at_sp(true); - - } else { - _navigator->set_can_loiter_at_sp(false); - } - _navigator->set_position_setpoint_triplet_updated(); }