aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator/rtl.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator/rtl.cpp')
-rw-r--r--src/modules/navigator/rtl.cpp178
1 files changed, 109 insertions, 69 deletions
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1b1d3f09..b7961a260 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -44,6 +44,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
+#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -52,6 +53,8 @@
#include "navigator.h"
#include "rtl.h"
+#define DELAY_SIGMA 0.01f
+
RTL::RTL(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
@@ -75,7 +78,11 @@ void
RTL::on_inactive()
{
_first_run = true;
- _rtl_state = RTL_STATE_NONE;
+
+ /* reset RTL state only if setpoint moved */
+ if (!_navigator->get_can_loiter_at_sp()) {
+ _rtl_state = RTL_STATE_NONE;
+ }
}
bool
@@ -84,14 +91,33 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
bool updated = false;
if (_first_run) {
+ _first_run = false;
+
+ /* 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_vstatus()->condition_landed) {
+ _rtl_state = RTL_STATE_LANDED;
+ mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+ /* 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;
+
+ /* 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(pos_sp_triplet);
updated = true;
- _first_run = false;
- }
- if ((_rtl_state == RTL_STATE_CLIMB
- || _rtl_state == RTL_STATE_RETURN)
- && is_mission_item_reached()) {
+ } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
@@ -106,31 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
/* make sure we have the latest params */
updateParams();
- /* 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_vstatus()->condition_landed) {
- _rtl_state = RTL_STATE_FINISHED;
- mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
- /* 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;
- /* otherwise go straight to return */
- } else {
- _rtl_state = RTL_STATE_RETURN;
- }
- }
-
- /* if switching directly to return state, set altitude setpoint to current altitude */
- if (_rtl_state == RTL_STATE_RETURN) {
- _mission_item.altitude_is_relative = false;
- _mission_item.altitude = _navigator->get_global_position()->alt;
- }
+ set_previous_pos_setpoint(pos_sp_triplet);
+ _navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
-
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
@@ -147,27 +153,28 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
- _navigator->set_is_in_loiter(false);
-
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
- case RTL_STATE_RETURN: {
+ case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
-
- /* TODO: add this again */
- // don't change altitude
- // if (_pos_sp_triplet.previous.valid) {
- // /* if previous setpoint is valid then use it to calculate heading to home */
- // _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
-
- // } else {
- // /* else use current position */
- // _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
- // }
+ // don't change altitude
+
+ if (pos_sp_triplet->previous.valid) {
+ /* if previous setpoint is valid then use it to calculate heading to home */
+ _mission_item.yaw = get_bearing_to_next_waypoint(
+ pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+ _mission_item.lat, _mission_item.lon);
+
+ } else {
+ /* else use current position */
+ _mission_item.yaw = get_bearing_to_next_waypoint(
+ _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+ _mission_item.lat, _mission_item.lon);
+ }
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -177,14 +184,12 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
- _navigator->set_is_in_loiter(false);
-
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
- case RTL_STATE_DESCEND: {
+ case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
@@ -194,23 +199,48 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _param_acceptance_radius.get();
- _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+ _mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
- _mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+ _mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
- _navigator->set_is_in_loiter(true);
-
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
- case RTL_STATE_LAND: {
+ case RTL_STATE_LOITER: {
+ bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
+ _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+ _mission_item.yaw = NAN;
+ _mission_item.loiter_radius = _navigator->get_loiter_radius();
+ _mission_item.loiter_direction = 1;
+ _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+ _mission_item.acceptance_radius = _param_acceptance_radius.get();
+ _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = autoland;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ _navigator->set_can_loiter_at_sp(true);
+
+ if (autoland) {
+ mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+ } else {
+ mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+ }
+ break;
+ }
+
+ case RTL_STATE_LAND: {
+ _mission_item.lat = _navigator->get_home_position()->lat;
+ _mission_item.lon = _navigator->get_home_position()->lon;
+ _mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
@@ -222,30 +252,38 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
- _navigator->set_is_in_loiter(false);
-
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
- case RTL_STATE_FINISHED: {
- /* nothing to do, report fail */
+ case RTL_STATE_LANDED: {
+ _mission_item.lat = _navigator->get_home_position()->lat;
+ _mission_item.lon = _navigator->get_home_position()->lon;
+ _mission_item.altitude_is_relative = false;
+ _mission_item.altitude = _navigator->get_home_position()->alt;
+ _mission_item.yaw = NAN;
+ _mission_item.loiter_radius = _navigator->get_loiter_radius();
+ _mission_item.loiter_direction = 1;
+ _mission_item.nav_cmd = NAV_CMD_IDLE;
+ _mission_item.acceptance_radius = _param_acceptance_radius.get();
+ _mission_item.time_inside = 0.0f;
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = true;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+ break;
}
default:
break;
}
- if (_rtl_state == RTL_STATE_FINISHED) {
- pos_sp_triplet->current.valid = false;
- pos_sp_triplet->next.valid = false;
- } else {
- /* if not finished, convert mission item to current position setpoint and make it valid */
- mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
- reset_mission_item_reached();
- pos_sp_triplet->current.valid = true;
- pos_sp_triplet->next.valid = false;
- }
+ /* convert mission item to current position setpoint and make it valid */
+ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+ reset_mission_item_reached();
+ pos_sp_triplet->current.valid = true;
+ pos_sp_triplet->next.valid = false;
}
void
@@ -262,18 +300,20 @@ RTL::advance_rtl()
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
- if (_param_land_delay.get() < 0) {
- _rtl_state = RTL_STATE_FINISHED;
+ if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+ _rtl_state = RTL_STATE_LOITER;
+
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
- case RTL_STATE_LAND:
- _rtl_state = RTL_STATE_FINISHED;
+ case RTL_STATE_LOITER:
+ _rtl_state = RTL_STATE_LAND;
break;
- case RTL_STATE_FINISHED:
+ case RTL_STATE_LAND:
+ _rtl_state = RTL_STATE_LANDED;
break;
default:
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 19:33:24 +0000