void
Delivery::return_home()
{
// turn on RTL with landing enabled
// should halt/switch to loiter if collision_imminent is true
// status = return ; change to complete
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
}
if (_rtl_state == RTL_STATE_LAND) {
_count++;
if (_count >= 3000) {
advance_rtl();
}
}
if (_rtl_state == RTL_STATE_LANDED) {
// Update Status now that return is complete
advance_delivery();
mavlink_log_critical(_navigator->get_mavlink_fd(), "Black Hawk Has Nested");
}
}
void
RTL::on_activation()
{
/* reset starting point so we override what the triplet contained from the previous navigation state */
_rtl_start_lock = false;
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item();
}
void
RTL::on_activation()
{
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item();
}
void
RTL::on_activation()
{
set_current_position_item(&_mission_item);
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->previous.valid = false;
pos_sp_triplet->next.valid = false;
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
+ get_rtl_altitude())) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
set_rtl_item();
}
void
RTL::on_active()
{
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
}
}
void
Navigator::on_mission_item_reached()
{
if (myState == NAV_STATE_MISSION) {
_mission.report_mission_item_reached();
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
} else {
/* advance by one mission item */
_mission.move_to_next();
}
if (_mission.current_mission_available()) {
set_mission_item();
} else {
/* if no more mission items available then finish mission */
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
request_loiter_or_ready();
}
} else if (myState == NAV_STATE_RTL) {
/* RTL completed */
if (_rtl_state == RTL_STATE_DESCEND) {
/* hovering above home position, land if needed or loiter */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
if (_mission_item.autocontinue) {
dispatch(EVENT_LAND_REQUESTED);
} else {
_reset_loiter_pos = false;
dispatch(EVENT_LOITER_REQUESTED);
}
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
set_rtl_item();
}
} else if (myState == NAV_STATE_LAND) {
/* landing completed */
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
_mission.publish_mission_result();
}
void
RTL::on_activation()
{
/* reset starting point so we override what the triplet contained from the previous navigation state */
_rtl_start_lock = false;
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* if lower than return altitude, climb up first */
} else if (home_dist > _param_rtl_min_dist.get()
&& _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item();
}
void
Navigator::start_rtl()
{
_do_takeoff = false;
/* decide if we need climb */
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
}
}
/* if switching directly to return state, reset altitude setpoint */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
_reset_loiter_pos = true;
set_rtl_item();
}
void
Delivery::set_delivery_items()
{
// set the parameters needed for getting to the destination
if (delivery_status == DELIV_ENROUTE) {
set_mission_items();
} else if (delivery_status == DELIV_DROPOFF) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "Setting descent mission item");
// setting the mission items for a descent to the _drop_alt
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false);
// mission_item to descend to _drop_alt
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
// .altitude needs to be referenced to location altitude, not home altitude
_mission_item.altitude = _navigator->get_home_position()->alt + _drop_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 10.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_critical(_navigator->get_mavlink_fd(), "DELIVERY: descend to %d m (%d m above destination)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
} else if (delivery_status == DELIV_RETURN) {
// setting the proper _rtl_state for current position
if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
set_rtl_item();
}
_first_run = false;
_complete = false;
}