fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 03:23:35 +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-05 18:05:09 +02:00
committed by Mathieu Bresciani
parent 81ee40eac8
commit 7c8fa82e76
3 changed files with 15 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+7 -3
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 home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_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; 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; rtl_altitude = gpos.alt;
} else if (cone_half_angle_deg <= 0) { } else if (cone_half_angle_deg <= 0) {
rtl_altitude = home.alt + _param_rtl_return_alt.get(); rtl_altitude = home.alt + _param_rtl_return_alt.get();
} else { } 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. // 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)); const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
src/modules/navigator/rtl.h
+1 -1
View File
@@ -94,7 +94,7 @@ private:
RTL_STATE_LANDED, RTL_STATE_LANDED,
} _rtl_state{RTL_STATE_NONE}; } _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}; bool _rtl_alt_min{false};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
src/modules/navigator/rtl_params.c
+7 -2
View File
@@ -125,7 +125,12 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
* @unit degrees * @unit degrees
* @min 0 * @min 0
* @max 90 * @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 * @group Return Mode
*/ */
PARAM_DEFINE_INT32(RTL_CONE_ANG, 30); PARAM_DEFINE_INT32(RTL_CONE_ANG, 0);