Skip to content

Commit

Permalink
VTOL_LAND: update state logic to explicitly set climbing beforehand i…
Browse files Browse the repository at this point in the history 
…f needed.
  • Loading branch information
@KonradRudin
KonradRudin committed Sep 18, 2023
1 parent a366810 commit 0cba351
Show file tree
Hide file tree
Showing 3 changed files with 145 additions and 115 deletions.
2 changes: 2 additions & 0 deletions src/modules/navigator/rtl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,6 +271,7 @@ void RTL::on_activation()
break;

case RtlType::VTOL_LAND:
_rtl_direct.setReturnAltMin(_enforce_rtl_alt);
_rtl_vtol_land.on_activation();
break;

Expand Down Expand Up @@ -342,6 +343,7 @@ void RTL::setRtlTypeAndDestination()
if (vtol_land) {
_rtl_type = RtlType::VTOL_LAND;
_rtl_vtol_land.setRtlApproach(rtl_position, chooseBestLandingApproach());
_rtl_vtol_land.setRtlAlt(rtl_alt);

} else {
_rtl_direct.setRtlAlt(rtl_alt);
Expand Down
243 changes: 130 additions & 113 deletions src/modules/navigator/vtol_land.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,9 +64,10 @@ VtolLand::on_activation()
{
_global_pos_sub.update();
_home_pos_sub.update();
_land_detected_sub.update();

if (!PX4_ISFINITE(_rtl_position.lat) || !PX4_ISFINITE(_rtl_position.lon)) {
// We don't have a valid rtl pposition, use the home position instead.
// We don't have a valid rtl position, use the home position instead.
_rtl_position.lat = _home_pos_sub.get().lat;
_rtl_position.lon = _home_pos_sub.get().lon;
_rtl_position.alt = _home_pos_sub.get().alt;
Expand All @@ -92,165 +93,181 @@ VtolLand::on_activation()
_land_approach.loiter_radius_m = _param_rtl_loiter_rad.get();
}

_land_state = vtol_land_state::MOVE_TO_LOITER;
if (_land_detected_sub.get().landed) {
// For safety reasons don't go into RTL if landed.
_land_state = vtol_land_state::IDLE;

set_loiter_position();
}
} else if ((_global_pos_sub.get().alt < _rtl_position.alt + _param_return_alt_rel_m.get()) || _enforce_rtl_alt) {
_land_state = vtol_land_state::CLIMBING;

void VtolLand::on_inactive()
{
_global_pos_sub.update();
_home_pos_sub.update();
} else {
_land_state = vtol_land_state::MOVE_TO_LOITER;
}

setRtlItem();
}

void
VtolLand::on_active()
{
_global_pos_sub.update();
_home_pos_sub.update();
_land_detected_sub.update();
_local_pos_sub.update();

if (is_mission_item_reached_or_completed()) {
switch (_land_state) {
case vtol_land_state::MOVE_TO_LOITER: {
_mission_item.altitude = _rtl_position.alt + _land_approach.height_m;
_mission_item.lat = _land_approach.lat;
_mission_item.lon = _land_approach.lon;
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
setRtlItem();
}
}

_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
void VtolLand::setRtlApproach(RtlPosition rtl_position, loiter_point_s loiter_pos)
{
if (!isActive()) {
_land_approach = loiter_pos;
_rtl_position = rtl_position;
}
}

// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
void VtolLand::setRtlItem()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

pos_sp_triplet->next.valid = true;
pos_sp_triplet->next.lat = _rtl_position.lat;
pos_sp_triplet->next.lon = _rtl_position.lon;
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND;
switch (_land_state) {
case vtol_land_state::CLIMBING: {
_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;
_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 = _navigator->get_loiter_radius();

mavlink_log_info(_navigator->get_mavlink_log_pub(), "VTOL RTL: climb to %d m (%d m above destination)\t",
(int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _rtl_position.alt));
events::send<int32_t, int32_t>(events::ID("vtol_rtl_climb_to"), events::Log::Info,
"VTOL RTL: climb to {1m_v} ({2m_v} above destination)",
(int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _rtl_position.alt));

_land_state = vtol_land_state::MOVE_TO_LOITER;
break;
}

pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;
case vtol_land_state::MOVE_TO_LOITER: {
_mission_item.lat = _land_approach.lat;
_mission_item.lon = _land_approach.lon;
_mission_item.altitude = math::max(_rtl_position.alt + _param_return_alt_rel_m.get(),
_global_pos_sub.get().alt);
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.force_heading = true;
_mission_item.autocontinue = false;
_mission_item.time_inside = _min_loiter_time_before_land;

_land_state = vtol_land_state::LOITER_DOWN;
break;
}
_mission_item.altitude_is_relative = false;

case vtol_land_state::LOITER_DOWN: {
_mission_item.lat = _rtl_position.lat;
_mission_item.lon = _rtl_position.lon;
_mission_item.altitude = _rtl_position.alt + _land_approach.height_m;
_mission_item.loiter_radius = _land_approach.loiter_radius_m;
_mission_item.origin = ORIGIN_ONBOARD;

_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.vtol_back_transition = true;
_navigator->set_can_loiter_at_sp(true);

struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

// set previous item location to loiter location such that vehicle tracks line between loiter
// location and land location after exiting the loiter circle
pos_sp_triplet->previous.lat = _land_approach.lat;
pos_sp_triplet->previous.lon = _land_approach.lon;
pos_sp_triplet->previous.alt = _mission_item.altitude;
pos_sp_triplet->previous.valid = true;
_land_state = vtol_land_state::LOITER_DOWN;

//publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
break;
}

reset_mission_item_reached();
case vtol_land_state::LOITER_DOWN: {
_mission_item.altitude = _rtl_position.alt + _land_approach.height_m;
_mission_item.lat = _land_approach.lat;
_mission_item.lon = _land_approach.lon;
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.loiter_radius = _param_rtl_loiter_rad.get();

_land_state = vtol_land_state::TRANSITION_TO_MC;
pos_sp_triplet->next.valid = true;
pos_sp_triplet->next.lat = _rtl_position.lat;
pos_sp_triplet->next.lon = _rtl_position.lon;
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND;

break;
}
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;

case vtol_land_state::TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
_mission_item.vtol_back_transition = true;
_land_state = vtol_land_state::MOVE_TO_LAND;
break;
}

issue_command(_mission_item);
case vtol_land_state::MOVE_TO_LAND: {
_mission_item.lat = _rtl_position.lat;
_mission_item.lon = _rtl_position.lon;
_mission_item.altitude = _rtl_position.alt + _land_approach.height_m;

struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.vtol_back_transition = true;

//publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
// set previous item location to loiter location such that vehicle tracks line between loiter
// location and land location after exiting the loiter circle
pos_sp_triplet->previous.lat = _land_approach.lat;
pos_sp_triplet->previous.lon = _land_approach.lon;
pos_sp_triplet->previous.alt = _mission_item.altitude;
pos_sp_triplet->previous.valid = true;

// issue_command(_mission_item);
reset_mission_item_reached();
_land_state = vtol_land_state::TRANSITION_TO_MC;

_land_state = vtol_land_state::LAND;
break;
}

break;
}
case vtol_land_state::TRANSITION_TO_MC: {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
_mission_item.vtol_back_transition = true;

case vtol_land_state::LAND: {
_mission_item.nav_cmd = NAV_CMD_LAND;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();

//publish_navigator_mission_item(); // for logging
_navigator->set_position_setpoint_triplet_updated();
_land_state = vtol_land_state::LAND;

// issue_command(_mission_item);
reset_mission_item_reached();
break;
}

_land_state = vtol_land_state::IDLE;
case vtol_land_state::LAND: {
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = _rtl_position.lat;
_mission_item.lon = _rtl_position.lon;
_mission_item.altitude = _rtl_position.alt;

break;
}
_land_state = vtol_land_state::IDLE;

default: {
break;
}

case vtol_land_state::IDLE: {
set_idle_item(&_mission_item);
_land_state = vtol_land_state::IDLE;

break;
}
break;
}
}
}

void VtolLand::setRtlApproach(RtlPosition rtl_position, loiter_point_s loiter_pos)
{
if (!isActive()) {
_land_approach = loiter_pos;
_rtl_position = rtl_position;
default: {

break;
}
}
}

void
VtolLand::set_loiter_position()
{
_mission_item.lat = _land_approach.lat;
_mission_item.lon = _land_approach.lon;
_mission_item.altitude = math::max(_rtl_position.alt + _param_return_alt_rel_m.get(),
_global_pos_sub.get().alt);
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.force_heading = true;
_mission_item.autocontinue = false;
_mission_item.time_inside = _min_loiter_time_before_land;

_mission_item.altitude_is_relative = false;

_mission_item.loiter_radius = _land_approach.loiter_radius_m;
_mission_item.origin = ORIGIN_ONBOARD;

_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();

// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

_navigator->set_can_loiter_at_sp(true);
// Execute command if set. This is required for commands like VTOL transition.
if (!MissionBlock::item_contains_position(_mission_item)) {
issue_command(_mission_item);
}

pos_sp_triplet->next.valid = false;
// Convert mission item to current position setpoint and make it valid.
mission_apply_limitation(_mission_item);

_navigator->set_position_setpoint_triplet_updated();
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
_navigator->set_position_setpoint_triplet_updated();
}
}

rtl_time_estimate_s VtolLand::calc_rtl_time_estimate()
Expand Down
15 changes: 13 additions & 2 deletions src/modules/navigator/vtol_land.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/wind.h>

#include <px4_platform_common/module_params.h>
Expand All @@ -63,17 +65,20 @@ class VtolLand : public MissionBlock, public ModuleParams

void on_activation() override;
void on_active() override;
void on_inactive() override;

void setRtlApproach(RtlPosition rtl_position, loiter_point_s loiter_pos);
void setReturnAltMin(bool min) { _enforce_rtl_alt = min; };
void setRtlAlt(float alt) {_rtl_alt = alt;};

rtl_time_estimate_s calc_rtl_time_estimate();

private:

enum class vtol_land_state {
MOVE_TO_LOITER = 0,
CLIMBING = 0,
MOVE_TO_LOITER,
LOITER_DOWN,
MOVE_TO_LAND,
TRANSITION_TO_MC,
LAND,
IDLE
Expand All @@ -82,13 +87,19 @@ class VtolLand : public MissionBlock, public ModuleParams
RtlPosition _rtl_position;
loiter_point_s _land_approach;

bool _enforce_rtl_alt{false};
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position

DEFINE_PARAMETERS(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_return_alt_rel_m,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad
)

void set_loiter_position();
void setRtlItem();

uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */
};

0 comments on commit 0cba351

Please sign in to comment.