dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

reworked RTL cone implementation 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-06-05 14:50:41 +03:00 committed by Lorenz Meier
parent dad5ce1e41
commit 29c3ce6d5d
2 changed files with 25 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -281,11 +281,9 @@ void RTL::set_rtl_item()
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Check if we are pretty close to the destination already.
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
const float descend_altitude_target = math::min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
// Compute the loiter altitude. const float loiter_altitude = math::min(descend_altitude_target, _rtl_alt);
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
switch (_rtl_state) { switch (_rtl_state) {
case RTL_STATE_CLIMB: { case RTL_STATE_CLIMB: {
@ -545,40 +543,32 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
// horizontal distance to destination // horizontal distance to destination
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
float rtl_altitude; // minium rtl altitude to use when outside of horizontal acceptance radius of target position.
// We choose the minimum height to be two times the distance from the land position in order to
// avoid the vehicle touching the ground while still moving horizontally.
const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f *
_navigator->get_acceptance_radius();
if (destination_dist <= _param_rtl_min_dist.get()) { float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get();
// we are very close to home, make sure we are above the RTL descend altitude
rtl_altitude = math::max(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
} else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { if (destination_dist <= _navigator->get_acceptance_radius()) {
rtl_altitude = gpos.alt; return_altitude_amsl = _destination.alt + 2.0f * destination_dist;
} else if (cone_half_angle_deg <= 0) {
rtl_altitude = _destination.alt + _param_rtl_return_alt.get();
} else { } else {
// constrain cone half angle to meaningful values. All other cases are already handled above. if (cone_half_angle_deg > 0.0f && destination_dist <= _param_rtl_min_dist.get()) {
const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
// minimum height above destination required // constrain cone half angle to meaningful values. All other cases are already handled above.
float height_above_destination_min = destination_dist / tanf(cone_half_angle_rad); const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
// minimum altitude we need in order to be within the user defined cone // minimum altitude we need in order to be within the user defined cone
const float altitude_min = math::constrain(height_above_destination_min + _destination.alt, _destination.alt, const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt;
_destination.alt + _param_rtl_return_alt.get());
if (gpos.alt < altitude_min) { return_altitude_amsl = math::min(cone_intersection_altitude_amsl, return_altitude_amsl);
rtl_altitude = altitude_min;
} else {
rtl_altitude = gpos.alt;
} }
return_altitude_amsl = math::max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
} }
// always demand altitude which is higher or equal the RTL descend altitude return math::max(return_altitude_amsl, gpos.alt);
rtl_altitude = math::max(rtl_altitude, _destination.alt + _param_rtl_descend_alt.get());
return rtl_altitude;
} }

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

@ -90,10 +90,11 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
/** /**
* Maximum horizontal distance from return destination, below which RTL_DESCEND_ALT is used as return altitude * Horizontal radius from return point within which special rules for return mode apply.
*
* The return altitude will be calculated based on RTL_CONE_ANG parameter.
* The yaw setpoint will switch to the one defined by corresponding waypoint.
* *
* If the vehicle is less than this horizontal distance from the return destination when return mode is activated it will ascend
* to RTL_DESCEND_ALT for the return journey (rather than the altitude set by RTL_RETURN_ALT and RTL_CONE_ANG).
* *
* @unit m * @unit m
* @min 0.5 * @min 0.5
@ -102,7 +103,7 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
* @increment 0.5 * @increment 0.5
* @group Return Mode * @group Return Mode
*/ */
PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f); PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
/** /**
* Return type * Return type
@ -134,7 +135,7 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
* @value 90 Only climb to at least RTL_DESCEND_ALT above destination. * @value 90 Only climb to at least RTL_DESCEND_ALT above destination.
* @group Return Mode * @group Return Mode
*/ */
PARAM_DEFINE_INT32(RTL_CONE_ANG, 0); PARAM_DEFINE_INT32(RTL_CONE_ANG, 45);
/** /**
* RTL precision land mode * RTL precision land mode