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

navigator : add support for precision landing

Browse Source
This commit is contained in:
Nicolas de Palezieux
2018-01-14 23:19:57 +05:30
committed by Lorenz Meier
parent 652d295b2d
commit aa4c6c038d
9 changed files with 925 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+49 -7
View File
@@ -188,7 +188,6 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
/* If we just completed a takeoff which was inserted before the right waypoint,
there is no need to report that we reached it because we didn't. */
if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
@@ -232,6 +231,17 @@ Mission::on_active()
do_abort_landing();
}
if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
// switch out of precision land once landed
if (_navigator->get_land_detected()->landed) {
_navigator->get_precland()->on_inactivation();
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
} else {
_navigator->get_precland()->on_active();
}
}
}
bool
@@ -501,9 +511,12 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
/* we have a new position item so set previous position setpoint to current */
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous = pos_sp_triplet->current;
/* we have a new position item so set previous position setpoint to current */
if (_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
pos_sp_triplet->previous = pos_sp_triplet->current;
}
/* do takeoff before going to setpoint if needed and not already in takeoff */
/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
@@ -681,14 +694,41 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = 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 == WORK_ITEM_TYPE_MOVE_TO_LAND &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* XXX: noop */
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = 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 */
@@ -760,8 +800,10 @@ Mission::set_mission_items()
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set current position setpoint from mission item (is protected against non-position items) */
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
/* issue command if ready (will do nothing for position mission items) */
issue_command(_mission_item);