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

navigator: don't execute land pattern if vtol in rotary wing mode

Browse Source 
- if vtol and in rotary wing mode then don't execute the mission landing
because it's designed to be flow as a fixed wing
- if vtol and in rotary wing mode and mission land is available then fly directly
to landing point and don't go home!

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-01-04 11:58:16 +01:00
committed by Lorenz Meier
parent 6c3f413379
commit b63f756745
6 changed files with 73 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+28 -9
View File
@@ -107,10 +107,28 @@ void RTL::find_RTL_destination()
_destination.type = RTL_DESTINATION_HOME;
const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
// 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();
double mission_landing_lat;
double mission_landing_lon;
float mission_landing_alt;
RTLDestinationType destination_type = RTL_DESTINATION_MISSION_LANDING;
if (vtol_in_rw_mode) {
mission_landing_lat = _navigator->get_mission_landing_lat();
mission_landing_lon = _navigator->get_mission_landing_lon();
mission_landing_alt = _navigator->get_mission_landing_alt();
destination_type = RTL_DESTINATION_HOME;
} else {
mission_landing_lat = _navigator->get_mission_landing_start_lat();
mission_landing_lon = _navigator->get_mission_landing_start_lon();
mission_landing_alt = _navigator->get_mission_landing_start_alt();
}
// compare home position to landing position to decide which is closer
dlat = mission_landing_lat - global_position.lat;
@@ -120,10 +138,11 @@ void RTL::find_RTL_destination()
// 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();
_destination.alt = _navigator->get_mission_landing_alt();
_destination.type = RTL_DESTINATION_MISSION_LANDING;
_destination.lat = mission_landing_lat;
_destination.lon = mission_landing_lon;
_destination.alt = mission_landing_alt;
_destination.type = destination_type;
}
}
@@ -198,6 +217,9 @@ void RTL::find_RTL_destination()
void RTL::on_activation()
{
_deny_mission_landing = _navigator->get_vstatus()->is_vtol
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
// output the correct message, depending on where the RTL destination is
switch (_destination.type) {
case RTL_DESTINATION_HOME:
@@ -213,8 +235,6 @@ void RTL::on_activation()
break;
}
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -316,7 +336,6 @@ void RTL::set_rtl_item()
}
case RTL_STATE_RETURN: {
// Don't change altitude.
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;