UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-26 16:28:06 +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-26 09:42:00 +02: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

120
src/modules/navigator/rtl_direct_mission_land.cpp
View File

@@ -140,9 +140,32 @@ void RtlDirectMissionLand::setActiveMissionItems()
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
} else if (item_contains_position(_mission_item)) {
static constexpr size_t max_num_next_items{1u};
int32_t next_mission_items_index[max_num_next_items];
size_t num_found_items = 0;
getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items);
mission_item_s next_mission_items[max_num_next_items];
const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
for (size_t i = 0U; i < num_found_items; i++) {
mission_item_s next_mission_item;
bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i],
reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT);
if (success) {
next_mission_items[i] = next_mission_item;
} else {
num_found_items = i;
break;
}
}
if (_mission_item.nav_cmd == NAV_CMD_LAND ||
_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
handleLanding(new_work_item_type);
handleLanding(new_work_item_type, next_mission_items, num_found_items);
} else {
// convert mission item to a simple waypoint, keep loiter to alt
@@ -156,21 +179,9 @@ void RtlDirectMissionLand::setActiveMissionItems()
pos_sp_triplet->previous = pos_sp_triplet->current;
}
int32_t next_mission_item_index;
size_t num_found_items = 0;
getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u);
if (num_found_items > 0) {
const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
mission_item_s next_mission_item;
bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
if (success) {
mission_apply_limitation(next_mission_item);
mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
}
mission_apply_limitation(next_mission_items[0u]);
mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
}
mission_apply_limitation(_mission_item);
@@ -192,85 +203,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
_navigator->set_position_setpoint_triplet_updated();
}
void RtlDirectMissionLand::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_MOVE_TO_LAND) {
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;
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
/* 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
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
} 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
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
}
}
}
bool RtlDirectMissionLand::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 RtlDirectMissionLand::calc_rtl_time_estimate()
{
rtl_time_estimate_s time_estimate;