fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtl+mission: remove do_need_move_to_land and handleLanding duplicated code to reduce flash

Browse Source
This commit is contained in:
Konrad
2023-10-31 14:16:02 +01:00
committed by Roman Bapst
parent 698c57c5f8
commit c1214c847f
13 changed files with 215 additions and 384 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+1 -146
View File
@@ -155,21 +155,6 @@ bool Mission::setNextMissionItem()
return (goToNextItem(true) == PX4_OK);
}
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()
{
@@ -188,7 +173,7 @@ Mission::do_need_move_to_takeoff()
void Mission::setActiveMissionItems()
{
/* Get mission item that comes after current if available */
constexpr static size_t max_num_next_items{2u};
static constexpr size_t max_num_next_items{2u};
int32_t next_mission_items_index[max_num_next_items];
size_t num_found_items;
@@ -218,10 +203,6 @@ void Mission::setActiveMissionItems()
const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
if (item_contains_position(_mission_item)) {
/* force vtol land */
if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
handleTakeoff(new_work_item_type, next_mission_items, num_found_items);
@@ -425,132 +406,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
}
}
void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
size_t &num_found_items)
{
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_TRANSITION_AFTER_TAKEOFF)
&& !_land_detected_sub.get().landed) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
num_found_items = 1u;
next_mission_items[0u] = _mission_item;
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;
}
/* 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) {
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;
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;
}
/* 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)) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
num_found_items = 1u;
next_mission_items[0u] = _mission_item;
/*
* Ignoring waypoint altitude:
* Set altitude to the same as we have now to prevent descending too fast into
* the ground. Actual landing will descend anyway until it touches down.
* XXX: We might want to change that at some point if it is clear to the user
* what the altitude means on this waypoint type.
*/
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;
// have to reset here because these field were used in set_vtol_transition_item
_mission_item.time_inside = 0.f;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
// 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 (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
} else { //_mission_item.land_precision == 2
_navigator->get_precland()->set_mode(PrecLandMode::Required);
}
_navigator->get_precland()->on_activation();
}
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
} else { //_mission_item.land_precision == 2
_navigator->get_precland()->set_mode(PrecLandMode::Required);
}
_navigator->get_precland()->on_activation();
}
}
/* ignore yaw for landing items */
/* XXX: if specified heading for landing is desired we could add another step before the descent
* that aligns the vehicle first */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
_mission_item.yaw = NAN;
}
}
void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
size_t &num_found_items)
{