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

Add parameter and logic to set RTL heading mode.

Browse Source
This commit is contained in:
mcsauder
2021-10-22 09:28:30 +02:00
committed by Mathieu Bresciani
parent 6938d24ec7
commit 9301288d1f
3 changed files with 106 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+72 -26
View File
@@ -346,6 +346,8 @@ void RTL::set_rtl_item()
const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
const float loiter_altitude = min(descend_altitude_target, _rtl_alt);
const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
@@ -362,7 +364,14 @@ void RTL::set_rtl_item()
_mission_item.lon = gpos.lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _navigator->get_local_position()->heading;
if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
@@ -384,14 +393,17 @@ void RTL::set_rtl_item()
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
// Use destination yaw if close to _destination.
// Check if we are pretty close to the destination already.
if (destination_dist < _param_rtl_min_dist.get()) {
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING &&
destination_dist > _param_rtl_min_dist.get()) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING ||
destination_dist < _param_rtl_min_dist.get()) {
// Use destination yaw if close to _destination.
_mission_item.yaw = _destination.yaw;
} else {
// Use current heading to _destination.
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -410,7 +422,6 @@ void RTL::set_rtl_item()
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
@@ -422,6 +433,9 @@ void RTL::set_rtl_item()
if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
@@ -449,20 +463,6 @@ void RTL::set_rtl_item()
case RTL_STATE_LOITER: {
const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON);
// Don't change altitude.
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _destination.yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t",
@@ -475,17 +475,46 @@ void RTL::set_rtl_item()
events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
}
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude; // Don't change altitude.
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
case RTL_STATE_HEAD_TO_CENTER: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
@@ -507,7 +536,17 @@ void RTL::set_rtl_item()
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
_mission_item.yaw = _destination.yaw;
} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.origin = ORIGIN_ONBOARD;
break;
@@ -518,9 +557,16 @@ void RTL::set_rtl_item()
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.yaw = _destination.yaw;
_mission_item.altitude = _destination.alt;
_mission_item.altitude_is_relative = false;
if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
_mission_item.yaw = _navigator->get_local_position()->heading;
} else {
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
src/modules/navigator/rtl.h
+21 -13
View File
@@ -59,6 +59,10 @@ class Navigator;
class RTL : public MissionBlock, public ModuleParams
{
public:
RTL(Navigator *navigator);
~RTL() = default;
enum RTLType {
RTL_HOME = 0,
RTL_LAND,
@@ -72,9 +76,11 @@ public:
RTL_DESTINATION_SAFE_POINT,
};
RTL(Navigator *navigator);
~RTL() = default;
enum RTLHeadingMode {
RTL_NAVIGATION_HEADING = 0,
RTL_DESTINATION_HEADING,
RTL_CURRENT_HEADING,
};
void on_inactivation() override;
void on_inactive() override;
@@ -148,21 +154,23 @@ private:
hrt_abstime _destination_check_time{0};
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
bool _rtl_alt_min{false};
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(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,
(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay,
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg,
(ParamFloat<px4::params::RTL_FLT_TIME>) _param_rtl_flt_time,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad
(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_rtl_land_delay,
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg,
(ParamFloat<px4::params::RTL_FLT_TIME>) _param_rtl_flt_time,
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad,
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md
)
// These need to point at different parameters depending on vehicle type.
@@ -173,7 +181,7 @@ private:
param_t _param_rtl_descent_speed{PARAM_INVALID};
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
uORB::Publication<rtl_flight_time_s> _rtl_flight_time_pub{ORB_ID(rtl_flight_time)};
uORB::Publication<rtl_flight_time_s> _rtl_flight_time_pub{ORB_ID(rtl_flight_time)};
};
float time_to_home(const matrix::Vector3f &to_home_vec,
src/modules/navigator/rtl_params.c
+13 -1
View File
@@ -156,7 +156,7 @@ PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15);
* @value 0 No precision landing
* @value 1 Opportunistic precision landing
* @value 2 Required precision landing
* @group Return To Land
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
@@ -173,3 +173,15 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
* @group Return Mode
*/
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
/**
* RTL heading mode
*
* Defines the heading behavior during RTL
*
* @value 0 Towards next waypoint.
* @value 1 Heading matches destination.
* @value 2 Use current heading.
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_HDG_MD, 0);