fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 16:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

for multicopter landings make sure that the copter moves

Browse Source 
to the landing waypoint first before the descending phase starts
This commit is contained in:
Roman 2016-01-16 17:22:06 +01:00 committed by tumbili
parent 871b4b482e
commit 0102e47708
2 changed files with 34 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -150,7 +150,6 @@ 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);
@ -181,6 +180,12 @@ 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) {

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

@ -92,9 +92,17 @@ MissionBlock::is_mission_item_reached()
}
return true;
}
case NAV_CMD_LAND:
return _navigator->get_vstatus()->condition_landed;
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;
/* TODO: count turns */
/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
@ -159,6 +167,12 @@ 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 ||
@ -222,6 +236,7 @@ MissionBlock::is_mission_item_reached()
return true;
}
}
return false;
}
@ -284,7 +299,17 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
break;
case NAV_CMD_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_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;
}
break;
case NAV_CMD_LOITER_TIME_LIMIT: