aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator')
-rw-r--r--src/modules/navigator/datalinkloss.cpp317
-rw-r--r--src/modules/navigator/datalinkloss.h95
-rw-r--r--src/modules/navigator/datalinkloss_params.c98
-rw-r--r--src/modules/navigator/module.mk4
-rw-r--r--src/modules/navigator/navigator.h4
-rw-r--r--src/modules/navigator/navigator_main.cpp13
-rw-r--r--src/modules/navigator/navigator_params.c11
7 files changed, 539 insertions, 3 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
new file mode 100644
index 000000000..17e85b284
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -0,0 +1,317 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file datalinkloss.cpp
+ * Helper class for Data Link Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA 0.01f
+
+DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
+ MissionBlock(navigator, name),
+ _rtl_state(RTL_STATE_NONE),
+ _param_return_alt(this, "RETURN_ALT"),
+ _param_descend_alt(this, "DESCEND_ALT"),
+ _param_land_delay(this, "LAND_DELAY")
+{
+ /* load initial params */
+ updateParams();
+ /* initial reset */
+ on_inactive();
+}
+
+DataLinkLoss::~DataLinkLoss()
+{
+}
+
+void
+DataLinkLoss::on_inactive()
+{
+ /* reset RTL state only if setpoint moved */
+ if (!_navigator->get_can_loiter_at_sp()) {
+ _rtl_state = RTL_STATE_NONE;
+ }
+}
+
+void
+DataLinkLoss::on_activation()
+{
+ /* 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();
+}
+
+void
+DataLinkLoss::on_active()
+{
+ if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
+ advance_rtl();
+ set_rtl_item();
+ }
+}
+
+void
+DataLinkLoss::set_rtl_item()
+{
+ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+ /* make sure we have the latest params */
+ updateParams();
+
+ set_previous_pos_setpoint();
+ _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;
+ _mission_item.lon = _navigator->get_global_position()->lon;
+ _mission_item.altitude_is_relative = false;
+ _mission_item.altitude = climb_alt;
+ _mission_item.yaw = NAN;
+ _mission_item.loiter_radius = _navigator->get_loiter_radius();
+ _mission_item.loiter_direction = 1;
+ _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _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: climb to %d meters above home",
+ (int)(climb_alt - _navigator->get_home_position()->alt));
+ break;
+ }
+
+ case RTL_STATE_RETURN: {
+ _mission_item.lat = _navigator->get_home_position()->lat;
+ _mission_item.lon = _navigator->get_home_position()->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;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _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: return at %d meters above home",
+ (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+ break;
+ }
+
+ 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;
+ _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 = NAV_CMD_LOITER_TIME_LIMIT;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _mission_item.time_inside = 0.0f;
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = false;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ 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_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 = _navigator->get_acceptance_radius();
+ _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", (double)_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();
+ _mission_item.loiter_direction = 1;
+ _mission_item.nav_cmd = NAV_CMD_LAND;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _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: land at home");
+ break;
+ }
+
+ 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 = _navigator->get_acceptance_radius();
+ _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;
+ }
+
+ reset_mission_item_reached();
+
+ /* convert mission item to current position setpoint and make it valid */
+ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+ pos_sp_triplet->next.valid = false;
+
+ _navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+DataLinkLoss::advance_rtl()
+{
+ switch (_rtl_state) {
+ case RTL_STATE_CLIMB:
+ _rtl_state = RTL_STATE_RETURN;
+ break;
+
+ case RTL_STATE_RETURN:
+ _rtl_state = RTL_STATE_DESCEND;
+ break;
+
+ case RTL_STATE_DESCEND:
+ /* only go to land if autoland is enabled */
+ 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_LOITER:
+ _rtl_state = RTL_STATE_LAND;
+ break;
+
+ case RTL_STATE_LAND:
+ _rtl_state = RTL_STATE_LANDED;
+ break;
+
+ default:
+ break;
+ }
+}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
new file mode 100644
index 000000000..242cfac8d
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.h
@@ -0,0 +1,95 @@
+/***************************************************************************
+ *
+ * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file datalinkloss.cpp
+ * Helper class for Data Link Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_DATALINKLOSS_H
+#define NAVIGATOR_DATALINKLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class DataLinkLoss : public MissionBlock
+{
+public:
+ DataLinkLoss(Navigator *navigator, const char *name);
+
+ ~DataLinkLoss();
+
+ virtual void on_inactive();
+
+ virtual void on_activation();
+
+ virtual void on_active();
+
+private:
+ /**
+ * Set the RTL item
+ */
+ void set_rtl_item();
+
+ /**
+ * Move to next RTL item
+ */
+ void advance_rtl();
+
+ enum RTLState {
+ RTL_STATE_NONE = 0,
+ RTL_STATE_CLIMB,
+ RTL_STATE_RETURN,
+ RTL_STATE_DESCEND,
+ RTL_STATE_LOITER,
+ RTL_STATE_LAND,
+ RTL_STATE_LANDED,
+ } _rtl_state;
+
+ control::BlockParamFloat _param_return_alt;
+ control::BlockParamFloat _param_descend_alt;
+ control::BlockParamFloat _param_land_delay;
+};
+
+#endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
new file mode 100644
index 000000000..bfe6ce7e1
--- /dev/null
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rtl_params.c
+ *
+ * Parameters for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * RTL parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter radius after RTL (FW only)
+ *
+ * Default value of loiter radius after RTL (fixedwing only).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
+
+/**
+ * RTL altitude
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 0
+ * @max 1
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+
+
+/**
+ * RTL loiter altitude
+ *
+ * Stay at this altitude above home position after RTL descending.
+ * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @min 0
+ * @max 100
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+
+/**
+ * RTL delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @min -1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index b50198996..a1e42ec38 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -49,7 +49,9 @@ SRCS = navigator_main.cpp \
offboard.cpp \
mission_feasibility_checker.cpp \
geofence.cpp \
- geofence_params.c
+ geofence_params.c \
+ datalinkloss.cpp \
+ datalinkloss_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 8edbb63b3..d0b2ed841 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -35,6 +35,7 @@
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_H
@@ -57,6 +58,7 @@
#include "loiter.h"
#include "rtl.h"
#include "offboard.h"
+#include "datalinkloss.h"
#include "geofence.h"
/**
@@ -165,6 +167,7 @@ private:
Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
Offboard _offboard; /**< class that handles offboard */
+ DataLinkLoss _dataLinkLoss; /**< class that handles offboard */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
@@ -173,6 +176,7 @@ private:
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
+ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */
/**
* Retrieve global position
*/
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ba46bd568..1ce6770c9 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -40,6 +40,7 @@
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -125,10 +126,12 @@ Navigator::Navigator() :
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_offboard(this, "OFF"),
+ _dataLinkLoss(this, "DLL"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
- _param_acceptance_radius(this, "ACC_RAD")
+ _param_acceptance_radius(this, "ACC_RAD"),
+ _param_datalinkloss_obc(this, "DLL_OBC")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@@ -376,7 +379,13 @@ Navigator::task_main()
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
- _navigation_mode = &_rtl; /* TODO: change this to something else */
+ /* Use complex data link loss mode only when enabled via param
+ * otherwise use rtl */
+ if (_param_datalinkloss_obc.get() != 0) {
+ _navigation_mode = &_dataLinkLoss;
+ } else {
+ _navigation_mode = &_rtl; /* TODO: change this to something else */
+ }
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 084afe340..afaf1c3c3 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -37,6 +37,7 @@
* Parameters for navigator in general
*
* @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -64,3 +65,13 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
+
+/**
+ * Set OBC mode for data link loss
+ *
+ * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
+ *
+ * @min 0
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-08 01:38:44 +0000