fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 10:29:07 +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 2020-10-27 15:08:11 +03:00 committed by Lorenz Meier
parent 6c3f413379
commit b63f756745
6 changed files with 73 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/modules/navigator/mission.cpp
View File

@ -391,6 +391,10 @@ Mission::find_mission_land_start()
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
_land_start_available = false;
bool found_land_start_marker = false;
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
@ -402,36 +406,41 @@ Mission::find_mission_land_start()
}
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
_land_start_available = true;
found_land_start_marker = true;
_land_start_index = i;
}
if (_land_start_available && i > _land_start_index && item_contains_position(missionitem)) {
if (found_land_start_marker && !_land_start_available && i > _land_start_index
&& item_contains_position(missionitem)) {
// use the position of any waypoint after the land start marker which specifies a position.
_landing_lat = missionitem.lat;
_landing_lon = missionitem.lon;
_landing_alt = missionitem.altitude_is_relative ? missionitem.altitude +
_navigator->get_home_position()->alt : missionitem.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)) {
_landing_start_lat = missionitem.lat;
_landing_start_lon = missionitem.lon;
_landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude +
_navigator->get_home_position()->alt : missionitem.altitude;
_land_start_available = true;
_land_start_index = i;
}
if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
(missionitem.nav_cmd == NAV_CMD_LAND)) {
_landing_lat = missionitem.lat;
_landing_lon = missionitem.lon;
_landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt :
missionitem.altitude;
return true;
// don't have a valid land start yet, use the landing item itself then
if (!_land_start_available) {
_land_start_index = i;
_landing_start_lat = _landing_lat;
_landing_start_lon = _landing_lon;
_landing_start_alt = _landing_alt;
_land_start_available = true;
}
}
}
_land_start_available = false;
return false;
return _land_start_available;
}
bool

8
src/modules/navigator/mission.h
View File

@ -87,6 +87,10 @@ public:
bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
bool get_mission_changed() const { return _mission_changed ; }
bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
double get_landing_start_lat() { return _landing_start_lat; }
double get_landing_start_lon() { return _landing_start_lon; }
float get_landing_start_alt() { return _landing_start_alt; }
double get_landing_lat() { return _landing_lat; }
double get_landing_lon() { return _landing_lon; }
float get_landing_alt() { return _landing_alt; }
@ -249,6 +253,10 @@ private:
// track location of planned mission landing
bool _land_start_available{false};
uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
double _landing_start_lat{0.0};
double _landing_start_lon{0.0};
float _landing_start_alt{0.0f};
double _landing_lat{0.0};
double _landing_lon{0.0};
float _landing_alt{0.0f};

5
src/modules/navigator/navigator.h
View File

@ -278,6 +278,10 @@ public:
bool start_mission_landing() { return _mission.land_start(); }
bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
int get_mission_landing_index() { return _mission.get_land_start_index(); }
double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
double get_mission_landing_lat() { return _mission.get_landing_lat(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); }
@ -399,6 +403,7 @@ private:
float _mission_cruising_speed_fw{-1.0f};
float _mission_throttle{NAN};
bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern
// if mission mode is inactive, this flag will be cleared after 2 seconds

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

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

37
src/modules/navigator/rtl.cpp
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;

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

@ -87,6 +87,8 @@ public:
bool getClimbAndReturnDone() { return _climb_and_return_done; }
bool denyMissionLanding() { return _deny_mission_landing; }
private:
/**
* Set the RTL item
@ -139,6 +141,7 @@ private:
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
bool _rtl_alt_min{false};
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};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,