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 cone angle: expose a few values to the user

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2019-07-03 13:53:17 +02:00 committed by Mathieu Bresciani
parent 81ee40eac8
commit 7c8fa82e76
3 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -355,17 +355,21 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
const home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
// horizontal distance to home position
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
float rtl_altitude;
if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
if (home_dist <= _param_rtl_min_dist.get()) {
rtl_altitude = home.alt + _param_rtl_descend_alt.get();
} else if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
rtl_altitude = gpos.alt;
} else if (cone_half_angle_deg <= 0) {
rtl_altitude = home.alt + _param_rtl_return_alt.get();
} else {
// horizontal distance to home position
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
// constrain cone half angle to meaningful values. All other cases are already handled above.
const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));

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

@ -94,7 +94,7 @@ private:
RTL_STATE_LANDED,
} _rtl_state{RTL_STATE_NONE};
float _rtl_alt; // AMSL altitude at which the vehicle should return to the home position
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
bool _rtl_alt_min{false};
DEFINE_PARAMETERS(

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

@ -125,7 +125,12 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
* @unit degrees
* @min 0
* @max 90
* @increment 1
* @value 0 No cone, always climb to RTL_RETURN_ALT above home.
* @value 25 25 degrees half cone angle.
* @value 45 45 degrees half cone angle.
* @value 65 65 degrees half cone angle.
* @value 80 80 degrees half cone angle.
* @value 90 Only climb to at least RTL_DESCEND_ALT above home.
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_CONE_ANG, 30);
PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);