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

[RTL] Update MAVLINK Mission logic to send optional loiter points to mission rally points.

Browse Source
This commit is contained in:
Konrad
2023-10-31 14:16:02 +01:00
committed by Roman Bapst
parent 26fd4c852c
commit e2cbf5be94
6 changed files with 21 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+11 -6
View File
@@ -357,18 +357,23 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
if (_safe_points_updated) {
for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) {
mission_safe_point_s mission_safe_point;
mission_item_s mission_safe_point;
bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq,
reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_safe_point_s), 500_ms);
sizeof(mission_item_s), 500_ms);
if (!success) {
PX4_ERR("dm_read failed");
continue;
}
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
// Ignore safepoints which are too close to the homepoint
const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
mission_safe_point.lat, mission_safe_point.lon);
if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
min_dist = dist;
@@ -397,7 +402,7 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_
}
void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
const mission_safe_point_s &mission_safe_point)
const mission_item_s &mission_safe_point)
{
// There is a safe point closer than home/mission landing
// TODO: handle all possible mission_safe_point.frame cases
@@ -405,14 +410,14 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
case 0: // MAV_FRAME_GLOBAL
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.alt;
rtl_position.alt = mission_safe_point.altitude;
rtl_position.yaw = _home_pos_sub.get().yaw;;
break;
case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home
rtl_position.alt = mission_safe_point.altitude + _home_pos_sub.get().alt; // alt of safe point is rel to home
rtl_position.yaw = _home_pos_sub.get().yaw;;
break;