diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cda6aaeef3..3dd2ce36d1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,6 +150,7 @@ void Mission::on_active() { check_mission_valid(); + /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -180,12 +181,6 @@ Mission::on_active() } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _waypoint_position_reached && _navigator->get_vstatus()->is_rotary_wing) { - // the copter has reached the landing spot location - // set the same landing item again but this time the vehicle will actually - // descend and land - set_mission_item_reached(); - set_mission_items(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 77362a9e2b..64512d3d3d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -92,17 +92,9 @@ MissionBlock::is_mission_item_reached() } return true; } + case NAV_CMD_LAND: - if (!_navigator->get_vstatus()->is_rotary_wing) { - return _navigator->get_vstatus()->condition_landed; - } else { - if (_waypoint_position_reached) { - // the copter has reached the position of the landing spot - // it can can start to descend - return _navigator->get_vstatus()->condition_landed; - } - } - break; + return _navigator->get_vstatus()->condition_landed; /* TODO: count turns */ /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ @@ -167,12 +159,6 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - // check if we have reached our landing spot - if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { - _waypoint_position_reached = true; - return false; - } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || @@ -236,7 +222,6 @@ MissionBlock::is_mission_item_reached() return true; } } - return false; } @@ -299,17 +284,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite break; case NAV_CMD_LAND: - - if (!_waypoint_position_reached) { - // we first need to move to the landing waypoint - // use the altitude specified in the mission item - sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; - sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } else { - sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; - sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; - } - + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: