yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rally Points: enable rally points always, except for RTL type == RTL_MISSION

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-11-22 08:18:25 +01:00
committed by Beat Küng
parent 5aaf1b100b
commit 5b235c34c2
3 changed files with 15 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/modules/navigator/rtl.cpp
View File

@@ -63,7 +63,7 @@ RTL::on_inactive()
}
void
RTL::find_closest_landing_point()
RTL::find_RTL_destination()
{
// get home position:
home_position_s &home_landing_position = *_navigator->get_home_position();
@@ -79,7 +79,8 @@ RTL::find_closest_landing_point()
_destination.type = RTL_DESTINATION_HOME;
if (_navigator->get_mission_start_land_available()) {
// consider the mission landing if not RTL_HOME type set
if (rtl_type() != RTL_HOME && _navigator->get_mission_start_land_available()) {
double mission_landing_lat = _navigator->get_mission_landing_lat();
double mission_landing_lon = _navigator->get_mission_landing_lon();
@@ -88,7 +89,8 @@ RTL::find_closest_landing_point()
dlon = mission_landing_lon - global_position.lon;
double dist_squared = dlat * dlat + dlon * dlon;
if (dist_squared < min_dist_squared) {
// set destination to mission landing if closest or in RTL_LAND or RTL_MISSION (so not in RTL_CLOSEST)
if (dist_squared < min_dist_squared || rtl_type() != RTL_CLOSEST) {
min_dist_squared = dist_squared;
_destination.lat = _navigator->get_mission_landing_lat();
_destination.lon = _navigator->get_mission_landing_lon();
@@ -98,6 +100,11 @@ RTL::find_closest_landing_point()
}
}
// do not consider rally point if RTL type is set to RTL_MISSION, so exit function and use either home or mission landing
if (rtl_type() == RTL_MISSION) {
return;
}
// compare to safe landing positions
mission_safe_point_s closest_safe_point {} ;
mission_stats_entry_s stats;
@@ -160,49 +167,6 @@ RTL::find_closest_landing_point()
}
void
RTL::find_RTL_destination()
{
// get home position:
home_position_s &home_landing_position = *_navigator->get_home_position();
switch (rtl_type()) {
case RTL_HOME:
// always take home position as landing destination
_destination.set(home_landing_position);
_destination.type = RTL_DESTINATION_HOME;
break;
case RTL_LAND:
// take mission landing as landing destination (if available), otherwise home
case RTL_MISSION:
// inverse mission
if (_navigator->get_mission_start_land_available()) {
_destination.lat = _navigator->get_mission_landing_lat();
_destination.lon = _navigator->get_mission_landing_lon();
_destination.alt = _navigator->get_mission_landing_alt();
_destination.type = RTL_DESTINATION_MISSION_LANDING;
} else {
_destination.set(home_landing_position);
_destination.type = RTL_DESTINATION_HOME;
}
break;
case RTL_CLOSEST:
// choose closest possible landing point (consider home, mission landing and safe points)
find_closest_landing_point();
break;
default:
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported RTL_TYPE. Change RTL_TYPE param.");
break;
}
}
int
RTL::rtl_type() const
{

1
src/modules/navigator/rtl.h
View File

@@ -74,7 +74,6 @@ public:
void on_activation() override;
void on_active() override;
void find_closest_landing_point();
void find_RTL_destination();
void set_return_alt_min(bool min);

11
src/modules/navigator/rtl_params.c
View File

@@ -107,13 +107,12 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
/**
* Return type
*
* Fly straight to the return location or planned mission landing and land there or
* use the planned mission to get to those points.
* Return mode destination and flight path (home location, rally point, mission landing pattern, reverse mission)
*
* @value 0 Return home via direct path
* @value 1 Return to a planned mission landing, if available, via direct path, else return to home via direct path
* @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path
* @value 3 Return via direct path to closest destination: home, mission landing pattern or safe point
* @value 0 Return to closest safe point (home or rally point) via direct path.
* @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path.
* @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points.
* @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_TYPE, 0);