Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Navigator: Re-initialize RTL every time it gets re-enabled

Browse Source
This commit is contained in:
Lorenz Meier
2017-02-25 18:50:16 +01:00
parent 6ce5807240
commit 7710b64d27
1 changed files with 16 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+16 -22
View File
@@ -80,10 +80,8 @@ RTL::~RTL()
void
RTL::on_inactive()
{
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
// reset RTL state
_rtl_state = RTL_STATE_NONE;
}
void
@@ -95,28 +93,24 @@ RTL::on_activation()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* if lower than return altitude, climb up first */
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
+ _param_return_alt.get())) {
_rtl_state = RTL_STATE_CLIMB;
} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
+ _param_return_alt.get())) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
set_rtl_item();