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

Safe Landing Points: Added new RTL_TYPE: closest between home, mission landing and safe points

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Co-authored by Martina Rivizzigno <martina@rivizzigno.it>
This commit is contained in:
Silvan Fuhrer
2019-10-28 09:54:59 +01:00
committed by Beat Küng
parent efafd91095
commit 0ce9329803
7 changed files with 224 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+24 -8
View File
@@ -376,17 +376,17 @@ Mission::set_execution_mode(const uint8_t mode)
bool
Mission::find_mission_land_start()
{
/* return true if a MAV_CMD_DO_LAND_START is found and internally save the index
/* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index
* return false if not found
*
* TODO: implement full spec and find closest landing point geographically
*/
const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;
struct mission_item_s missionitem = {};
struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START
for (size_t i = 0; i < _mission.count; i++) {
struct mission_item_s missionitem = {};
for (size_t i = 1; i < _mission.count; i++) {
const ssize_t len = sizeof(missionitem);
missionitem_prev = missionitem; // store the last mission item before reading a new one
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -394,11 +394,27 @@ Mission::find_mission_land_start()
break;
}
if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) ||
((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
(missionitem.nav_cmd == NAV_CMD_LAND)) {
// first check for DO_LAND_START marker
if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) && (missionitem_prev.nav_cmd == NAV_CMD_WAYPOINT)) {
_land_start_available = true;
_land_start_index = i;
// the DO_LAND_START marker contains no position sp, so take them from the previous mission item
_landing_lat = missionitem_prev.lat;
_landing_lon = missionitem_prev.lon;
_landing_alt = missionitem_prev.altitude;
return true;
// if no DO_LAND_START marker available, also check for VTOL_LAND or normal LAND
} else if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
(missionitem.nav_cmd == NAV_CMD_LAND)) {
_land_start_available = true;
_land_start_index = i;
_landing_lat = missionitem.lat;
_landing_lon = missionitem.lon;
_landing_alt = missionitem.altitude;
return true;
}
}