dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
src/modules/navigator/mission.cpp
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)
{

7
src/modules/navigator/mission.h
View File

@ -73,11 +73,6 @@ private:
bool setNextMissionItem() override;
/**
* 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
*/
@ -97,8 +92,6 @@ private:
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);

123
src/modules/navigator/mission_base.cpp
View File

@ -446,6 +446,11 @@ MissionBase::set_mission_items()
/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
loadCurrentMissionItem();
/* force vtol land */
if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
setActiveMissionItems();
} else {
@ -517,6 +522,124 @@ MissionBase::set_mission_result()
_navigator->set_mission_result_updated();
}
bool MissionBase::do_need_move_to_item()
{
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();
}
void MissionBase::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();
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) &&
(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) &&
!_land_detected_sub.get().landed;
/* move to land wp as fixed wing */
if (needs_vtol_landing) {
if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
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;
_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);
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_item() &&
(_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
_navigator->reset_position_setpoint(pos_sp_triplet->previous);
} else {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
startPrecLand(_mission_item.land_precision);
}
}
}
/* 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;
}
}
bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const
{
return ((p1->valid == p2->valid) &&

18
src/modules/navigator/mission_base.h
View File

@ -279,6 +279,24 @@ protected:
*
*/
void publish_navigator_mission_item();
/**
* @brief Do need move to item
*
* @return true if the item is horizontally further away than the mission item
* @return false otherwise
*/
bool do_need_move_to_item();
/**
* @brief Handle landing
*
* @param new_work_item_type new work item type state machine to be set
* @param next_mission_items the next mission items after the current mission item
* @param num_found_items number of found next mission items
*/
void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
size_t &num_found_items);
/**
* @brief I position setpoint equal
*

12
src/modules/navigator/mission_block.cpp
View File

@ -1059,3 +1059,15 @@ float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, Headi
return desired_yaw;
}
void MissionBlock::startPrecLand(uint16_t land_precision)
{
if (_mission_item.land_precision == 1) {
_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
_navigator->get_precland()->on_activation();
} else { //_mission_item.land_precision == 2
_navigator->get_precland()->set_mode(PrecLandMode::Required);
_navigator->get_precland()->on_activation();
}
}

2
src/modules/navigator/mission_block.h
View File

@ -223,6 +223,8 @@ protected:
float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
void startPrecLand(uint16_t land_precision);
/**
* @brief Issue a command for mission items with a nav_cmd that specifies an action
*

9
src/modules/navigator/rtl_direct.cpp
View File

@ -322,14 +322,7 @@ void RtlDirect::set_rtl_item()
_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();
}
startPrecLand(_mission_item.land_precision);
_rtl_state = RTLState::IDLE;

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;

2
src/modules/navigator/rtl_direct_mission_land.h
View File

@ -65,8 +65,6 @@ public:
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
bool _needs_climbing{false}; //< Flag if climbing is required at the start
bool _enforce_rtl_alt{false};

134
src/modules/navigator/rtl_mission_fast.cpp
View File

@ -54,6 +54,8 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) :
void RtlMissionFast::on_activation()
{
_home_pos_sub.update();
_is_current_planned_mission_item_valid = 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()) == PX4_OK;
@ -65,18 +67,6 @@ void RtlMissionFast::on_activation()
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();
}
bool RtlMissionFast::setNextMissionItem()
{
return (goToNextPositionItem(true) == PX4_OK);
@ -100,9 +90,32 @@ void RtlMissionFast::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
@ -116,21 +129,11 @@ void RtlMissionFast::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);
@ -152,85 +155,6 @@ void RtlMissionFast::setActiveMissionItems()
_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;
_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 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;

4
src/modules/navigator/rtl_mission_fast.h
View File

@ -56,16 +56,12 @@ public:
~RtlMissionFast() = default;
void on_activation() override;
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate() override;
private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};

19
src/modules/navigator/rtl_mission_fast_reverse.cpp
View File

@ -54,6 +54,8 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
void RtlMissionFastReverse::on_activation()
{
_home_pos_sub.update();
_is_current_planned_mission_item_valid = 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()) == PX4_OK;
@ -71,12 +73,6 @@ void RtlMissionFastReverse::on_active()
MissionBase::on_active();
}
void RtlMissionFastReverse::on_inactive()
{
_home_pos_sub.update();
MissionBase::on_inactive();
}
bool RtlMissionFastReverse::setNextMissionItem()
{
return (goToPreviousPositionItem(true) == PX4_OK);
@ -227,7 +223,7 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
_mission_item.yaw = NAN;
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
do_need_move_to_land()) {
do_need_move_to_item()) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
_mission_item.altitude = _global_pos_sub.get().alt;
@ -249,15 +245,6 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
}
}
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;

2
src/modules/navigator/rtl_mission_fast_reverse.h
View File

@ -57,7 +57,6 @@ public:
void on_activation() override;
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate() override;
@ -65,7 +64,6 @@ private:
bool setNextMissionItem() override;
void setActiveMissionItems() override;
void handleLanding(WorkItemType &new_work_item_type);
bool do_need_move_to_land();
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
};