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

RTL: replace deny_mission_landing logic by getDestinationTypeMissionLanding()

Browse Source 
The rtl instance decides on destination of RTL, and Navigator then switches to
mission mode if required.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-22 17:28:34 +02:00 committed by Roman Bapst
parent ab08bbdb58
commit f4f9b8ebf2
3 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/navigator/navigator_main.cpp
View File

@ -589,8 +589,8 @@ Navigator::run()
}
if (!rtl_activated && !_rtl.denyMissionLanding() && _rtl.getClimbAndReturnDone()
&& get_mission_start_land_available()) {
if (!rtl_activated && _rtl.getClimbAndReturnDone()
&& _rtl.getDestinationTypeMissionLanding()) {
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED

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

@ -138,7 +138,7 @@ void RTL::find_RTL_destination()
double dist_squared = coord_dist_sq(dlat, dlon);
// set destination to mission landing if closest or in RTL_TYPE_MISSION_LANDING or RTL_TYPE_MISSION_LANDING_REVERSED (so not in RTL_TYPE_CLOSEST)
if (dist_squared < min_dist_squared || _param_rtl_type.get() != RTL_TYPE_CLOSEST) {
if (dist_squared < min_dist_squared || (_param_rtl_type.get() != RTL_TYPE_CLOSEST && !vtol_in_rw_mode)) {
min_dist_squared = dist_squared;
_destination.lat = mission_landing_lat;
_destination.lon = mission_landing_lon;
@ -235,10 +235,6 @@ 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:

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

@ -97,10 +97,11 @@ public:
bool getClimbAndReturnDone() { return _climb_and_return_done; }
bool denyMissionLanding() { return _deny_mission_landing; }
void get_rtl_xy_z_speed(float &xy, float &z);
matrix::Vector2f get_wind();
bool getDestinationTypeMissionLanding() { return _destination.type == RTL_DESTINATION_MISSION_LANDING; }
private:
void set_rtl_item();
@ -149,7 +150,6 @@ private:
float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending
bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state
bool _deny_mission_landing{false};
bool _rtl_alt_min{false};
DEFINE_PARAMETERS(