aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/modules/commander/commander.cpp53
-rw-r--r--src/modules/commander/commander_params.c25
-rw-r--r--src/modules/commander/state_machine_helper.cpp22
-rw-r--r--src/modules/commander/state_machine_helper.h2
-rw-r--r--src/modules/navigator/datalinkloss.cpp188
-rw-r--r--src/modules/navigator/datalinkloss.h97
-rw-r--r--src/modules/navigator/datalinkloss_params.c136
-rw-r--r--src/modules/navigator/enginefailure.cpp147
-rw-r--r--src/modules/navigator/enginefailure.h83
-rw-r--r--src/modules/navigator/mission.cpp32
-rw-r--r--src/modules/navigator/mission.h8
-rw-r--r--src/modules/navigator/module.mk5
-rw-r--r--src/modules/navigator/navigator.h20
-rw-r--r--src/modules/navigator/navigator_main.cpp49
-rw-r--r--src/modules/navigator/navigator_mode.cpp3
-rw-r--r--src/modules/navigator/navigator_params.c11
-rw-r--r--src/modules/uORB/topics/mission_result.h1
-rw-r--r--src/modules/uORB/topics/position_setpoint_triplet.h1
-rw-r--r--src/modules/uORB/topics/vehicle_status.h8
19 files changed, 838 insertions, 53 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4f976546e..c6f56d5e3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -127,7 +127,6 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
-#define DL_TIMEOUT 5 * 1000* 1000
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
@@ -652,6 +651,8 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
+ param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
+ param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
/* welcome user */
warnx("starting");
@@ -844,11 +845,13 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to telemetry status topics */
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+ uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM];
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_last_heartbeat[i] = 0;
+ telemetry_last_dl_loss[i] = 0;
telemetry_lost[i] = true;
}
@@ -932,6 +935,8 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t arming_ret;
int32_t datalink_loss_enabled = false;
+ int32_t datalink_loss_timeout = 10;
+ int32_t datalink_regain_timeout = 0;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
@@ -992,6 +997,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
+ param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
+ param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
}
orb_check(sp_man_sub, &updated);
@@ -1032,7 +1039,7 @@ int commander_thread_main(int argc, char *argv[])
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
- hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+ hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
(void)rc_calibration_check(mavlink_fd);
}
@@ -1132,6 +1139,22 @@ int commander_thread_main(int argc, char *argv[])
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
+ if (gps_position.fix_type >= 3 && //XXX check eph and epv ?
+ hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
+ /* handle the case where gps was regained */
+ if (status.gps_failure) {
+ status.gps_failure = false;
+ status_changed = true;
+ mavlink_log_critical(mavlink_fd, "gps regained");
+ }
+ } else {
+ if (!status.gps_failure) {
+ status.gps_failure = true;
+ status_changed = true;
+ mavlink_log_critical(mavlink_fd, "gps fix lost");
+ }
+ }
+
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
@@ -1257,6 +1280,14 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+
+ /* Check for geofence violation */
+ if (pos_sp_triplet.geofence_violated) {
+ //XXX: make this configurable to select different actions (e.g. navigation modes)
+ /* this will only trigger if geofence is activated via param and a geofence file is present */
+ armed.force_failsafe = true;
+ status_changed = true;
+ } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
@@ -1453,15 +1484,21 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
- if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
- /* handle the case where data link was regained */
- if (telemetry_lost[i]) {
+ if (telemetry_last_heartbeat[i] != 0 &&
+ hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
+ /* handle the case where data link was regained,
+ * accept datalink as healthy only after datalink_regain_timeout seconds
+ * */
+ if (telemetry_lost[i] &&
+ hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
+
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
+ have_link = true;
}
- have_link = true;
} else {
+ telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true;
@@ -1480,6 +1517,7 @@ int commander_thread_main(int argc, char *argv[])
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
+ status.data_link_lost_counter++;
status_changed = true;
}
}
@@ -1538,7 +1576,8 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
- mission_result.finished);
+ mission_result.finished,
+ mission_result.stay_in_failsafe);
// TODO handle mode changes by commands
if (main_state_changed) {
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index dba68700b..3d1e231c6 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -104,4 +104,27 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
* @min 0
* @max 1
*/
-PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
+PARAM_DEFINE_INT32(DL_LOSS_EN, 0);
+
+ /** Datalink loss time threshold
+ *
+ * After this amount of seconds without datalink the data link lost mode triggers
+ *
+ * @group commander
+ * @unit second
+ * @min 0
+ * @max 30
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
+
+/** Datalink regain time threshold
+ *
+ * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
+ * flag is set back to false
+ *
+ * @group commander
+ * @unit second
+ * @min 0
+ * @max 30
+ */
+PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f8589d24b..ecfe62e03 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -436,7 +436,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+ const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -490,9 +491,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
+ * - if we have an engine failure
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
- if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+ if (status->engine_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+ } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
@@ -532,8 +536,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_LOITER:
- /* go into failsafe if datalink and RC is lost */
- if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
+ /* go into failsafe on a engine failure or if datalink and RC is lost */
+ if (status->engine_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+ } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -586,8 +592,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_RTL:
- /* require global position and home */
- if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+ /* require global position and home, also go into failsafe on an engine failure */
+
+ if (status->engine_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+ } else if ((!status->condition_global_position_valid ||
+ !status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 69ce8bbce..61d0f29d0 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
new file mode 100644
index 000000000..19f335633
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ * 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),
+ _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100),
+ _param_commsholdwaittime(this, "CH_T"),
+ _param_commsholdlat(this, "CH_LAT"),
+ _param_commsholdlon(this, "CH_LON"),
+ _param_commsholdalt(this, "CH_ALT"),
+ _param_airfieldhomelat(this, "AH_LAT"),
+ _param_airfieldhomelon(this, "AH_LON"),
+ _param_airfieldhomealt(this, "AH_ALT"),
+ _param_numberdatalinklosses(this, "DLL_N"),
+ _dll_state(DLL_STATE_NONE)
+{
+ /* 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()) {
+ _dll_state = DLL_STATE_NONE;
+ }
+}
+
+void
+DataLinkLoss::on_activation()
+{
+ _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+ set_dll_item();
+}
+
+void
+DataLinkLoss::on_active()
+{
+ if (is_mission_item_reached()) {
+ advance_dll();
+ set_dll_item();
+ }
+}
+
+void
+DataLinkLoss::set_dll_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 (_dll_state) {
+ case DLL_STATE_FLYTOCOMMSHOLDWP: {
+ _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
+ _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
+ _mission_item.altitude_is_relative = false;
+ _mission_item.altitude = _param_commsholdalt.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 = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = true;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ _navigator->set_can_loiter_at_sp(true);
+ break;
+ }
+ case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
+ _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
+ _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
+ _mission_item.altitude_is_relative = false;
+ _mission_item.altitude = _param_airfieldhomealt.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_UNLIMITED;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = true;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ _navigator->set_can_loiter_at_sp(true);
+ 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_dll()
+{
+ switch (_dll_state) {
+ case DLL_STATE_NONE:
+ /* Check the number of data link losses. If above home fly home directly */
+ updateSubscriptions();
+ if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) {
+ _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+ } else {
+ _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+ }
+ break;
+ case DLL_STATE_FLYTOCOMMSHOLDWP:
+ //XXX check here if time is over are over
+ _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+ _navigator->get_mission_result()->stay_in_failsafe = true;
+ _navigator->publish_mission_result();
+ break;
+ default:
+ break;
+ }
+}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
new file mode 100644
index 000000000..5a46b5700
--- /dev/null
+++ b/src/modules/navigator/datalinkloss.h
@@ -0,0 +1,97 @@
+/***************************************************************************
+ *
+ * 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.h
+ * 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/Subscription.hpp>
+
+#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:
+ /* Subscriptions */
+ uORB::Subscription<vehicle_status_s> _vehicleStatus;
+
+ /* Params */
+ control::BlockParamFloat _param_commsholdwaittime;
+ control::BlockParamInt _param_commsholdlat; // * 1e7
+ control::BlockParamInt _param_commsholdlon; // * 1e7
+ control::BlockParamFloat _param_commsholdalt;
+ control::BlockParamInt _param_airfieldhomelat; // * 1e7
+ control::BlockParamInt _param_airfieldhomelon; // * 1e7
+ control::BlockParamFloat _param_airfieldhomealt;
+ control::BlockParamInt _param_numberdatalinklosses;
+
+ enum DLLState {
+ DLL_STATE_NONE = 0,
+ DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+ DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+ } _dll_state;
+
+ /**
+ * Set the DLL item
+ */
+ void set_dll_item();
+
+ /**
+ * Move to next DLL item
+ */
+ void advance_dll();
+
+};
+#endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
new file mode 100644
index 000000000..038c80a1a
--- /dev/null
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ * 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 datalinkloss_params.c
+ *
+ * Parameters for DLL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Data Link Loss parameters, accessible via MAVLink
+ */
+
+/**
+ * Comms hold wait time
+ *
+ * The amount of time in seconds the system should wait at the comms hold waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
+
+/**
+ * Comms hold Lat
+ *
+ * Latitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120);
+
+/**
+ * Comms hold Lon
+ *
+ * Longitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
+
+/**
+ * Comms hold alt
+ *
+ * Altitude of comms hold waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
+
+/**
+ * Airfield home Lat
+ *
+ * Latitude of airfield home waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810);
+
+/**
+ * Airfield home Lon
+ *
+ * Longitude of airfield home waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250);
+
+/**
+ * Airfield home alt
+ *
+ * Altitude of airfield home waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f);
+
+/**
+ * Number of allowed Datalink timeouts
+ *
+ * After more than this number of data link timeouts the aircraft returns home directly
+ *
+ * @group commander
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(NAV_DLL_N, 2);
diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp
new file mode 100644
index 000000000..de567f0dc
--- /dev/null
+++ b/src/modules/navigator/enginefailure.cpp
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ * 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 enginefailure.cpp
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @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 "navigator.h"
+#include "enginefailure.h"
+
+#define DELAY_SIGMA 0.01f
+
+EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
+ MissionBlock(navigator, name),
+ _ef_state(EF_STATE_NONE)
+{
+ /* load initial params */
+ updateParams();
+ /* initial reset */
+ on_inactive();
+}
+
+EngineFailure::~EngineFailure()
+{
+}
+
+void
+EngineFailure::on_inactive()
+{
+ _ef_state = EF_STATE_NONE;
+}
+
+void
+EngineFailure::on_activation()
+{
+ _ef_state = EF_STATE_LOITERDOWN;
+ set_ef_item();
+}
+
+void
+EngineFailure::on_active()
+{
+ if (is_mission_item_reached()) {
+ advance_ef();
+ set_ef_item();
+ }
+}
+
+void
+EngineFailure::set_ef_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 (_ef_state) {
+ case EF_STATE_LOITERDOWN: {
+ //XXX create mission item at ground (below?) here
+
+ _mission_item.lat = _navigator->get_global_position()->lat;
+ _mission_item.lon = _navigator->get_global_position()->lon;
+ _mission_item.altitude_is_relative = false;
+ //XXX setting altitude to a very low value, evaluate other options
+ _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
+ _mission_item.yaw = NAN;
+ _mission_item.loiter_radius = _navigator->get_loiter_radius();
+ _mission_item.loiter_direction = 1;
+ _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+ _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+ _mission_item.pitch_min = 0.0f;
+ _mission_item.autocontinue = true;
+ _mission_item.origin = ORIGIN_ONBOARD;
+
+ _navigator->set_can_loiter_at_sp(true);
+ 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
+EngineFailure::advance_ef()
+{
+ switch (_ef_state) {
+ case EF_STATE_NONE:
+ _ef_state = EF_STATE_LOITERDOWN;
+ break;
+ default:
+ break;
+ }
+}
diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h
new file mode 100644
index 000000000..2c48c2fce
--- /dev/null
+++ b/src/modules/navigator/enginefailure.h
@@ -0,0 +1,83 @@
+/***************************************************************************
+ *
+ * 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 enginefailure.h
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_ENGINEFAILURE_H
+#define NAVIGATOR_ENGINEFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class EngineFailure : public MissionBlock
+{
+public:
+ EngineFailure(Navigator *navigator, const char *name);
+
+ ~EngineFailure();
+
+ virtual void on_inactive();
+
+ virtual void on_activation();
+
+ virtual void on_active();
+
+private:
+ enum EFState {
+ EF_STATE_NONE = 0,
+ EF_STATE_LOITERDOWN = 1,
+ } _ef_state;
+
+ /**
+ * Set the DLL item
+ */
+ void set_ef_item();
+
+ /**
+ * Move to next EF item
+ */
+ void advance_ef();
+
+};
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c0e37a3ed..c76192f66 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff(false),
- _mission_result_pub(-1),
- _mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
@@ -587,18 +585,18 @@ void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
- _mission_result.reached = true;
- _mission_result.seq_reached = _current_offboard_mission_index;
+ _navigator->get_mission_result()->reached = true;
+ _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
}
- publish_mission_result();
+ _navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
- _mission_result.seq_current = _current_offboard_mission_index;
- publish_mission_result();
+ _navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
+ _navigator->publish_mission_result();
save_offboard_mission_state();
}
@@ -606,23 +604,7 @@ Mission::report_current_offboard_mission_item()
void
Mission::report_mission_finished()
{
- _mission_result.finished = true;
- publish_mission_result();
+ _navigator->get_mission_result()->finished = true;
+ _navigator->publish_mission_result();
}
-void
-Mission::publish_mission_result()
-{
- /* lazily publish the mission result only once available */
- if (_mission_result_pub > 0) {
- /* publish mission result */
- orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
-
- } else {
- /* advertise and publish */
- _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
- }
- /* reset reached bool */
- _mission_result.reached = false;
- _mission_result.finished = false;
-}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4da6a1155..1b8c8c874 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -128,11 +128,6 @@ private:
*/
void report_mission_finished();
- /**
- * Publish the mission result so commander and mavlink know what is going on
- */
- void publish_mission_result();
-
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
@@ -145,9 +140,6 @@ private:
bool _need_takeoff;
bool _takeoff;
- orb_advert_t _mission_result_pub;
- struct mission_result_s _mission_result;
-
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index b50198996..0539087df 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -49,7 +49,10 @@ SRCS = navigator_main.cpp \
offboard.cpp \
mission_feasibility_checker.cpp \
geofence.cpp \
- geofence_params.c
+ geofence_params.c \
+ datalinkloss.cpp \
+ enginefailure.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..fe6639dfe 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
@@ -51,12 +52,15 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/mission_result.h>
#include "navigator_mode.h"
#include "mission.h"
#include "loiter.h"
#include "rtl.h"
#include "offboard.h"
+#include "datalinkloss.h"
+#include "enginefailure.h"
#include "geofence.h"
/**
@@ -101,6 +105,11 @@ public:
void load_fence_from_file(const char *filename);
/**
+ * Publish the mission result so commander and mavlink know what is going on
+ */
+ void publish_mission_result();
+
+ /**
* Setters
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
@@ -113,7 +122,9 @@ public:
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct home_position_s* get_home_position() { return &_home_pos; }
- struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+ struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+ struct mission_result_s* get_mission_result() { return &_mission_result; }
+
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
@@ -141,6 +152,7 @@ private:
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
+ orb_advert_t _mission_result_pub;
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -150,6 +162,8 @@ private:
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
+ mission_result_s _mission_result;
+
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -165,6 +179,9 @@ private:
Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
Offboard _offboard; /**< class that handles offboard */
+ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
+ EngineFailure _engineFailure; /**< class that handles the engine failure mode
+ (FW only!) */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
@@ -173,6 +190,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 331a9a728..043d883b2 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>
@@ -107,6 +108,7 @@ Navigator::Navigator() :
_offboard_mission_sub(-1),
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
+ _mission_result_pub(-1),
_vstatus{},
_control_mode{},
_global_pos{},
@@ -114,6 +116,7 @@ Navigator::Navigator() :
_mission_item{},
_nav_caps{},
_pos_sp_triplet{},
+ _mission_result{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
@@ -125,10 +128,13 @@ Navigator::Navigator() :
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_offboard(this, "OFF"),
+ _dataLinkLoss(this, "DLL"),
+ _engineFailure(this, "EF"),
_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;
@@ -335,6 +341,11 @@ Navigator::task_main()
/* Check geofence violation */
if (!_geofence.inside(&_global_pos)) {
+ /* inform other apps via the sp triplet */
+ _pos_sp_triplet.geofence_violated = true;
+ if (_pos_sp_triplet.geofence_violated != true) {
+ _pos_sp_triplet_updated = true;
+ }
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -342,6 +353,11 @@ Navigator::task_main()
_geofence_violation_warning_sent = true;
}
} else {
+ /* inform other apps via the sp triplet */
+ _pos_sp_triplet.geofence_violated = false;
+ if (_pos_sp_triplet.geofence_violated != false) {
+ _pos_sp_triplet_updated = true;
+ }
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -357,6 +373,9 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
+ /* Some failsafe modes prohibit the fallback to mission
+ * usually this is done after some time to make sure
+ * that the full failsafe operation is performed */
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
@@ -366,7 +385,16 @@ 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_AUTO_LANDENGFAIL:
+ _navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
@@ -534,3 +562,20 @@ int navigator_main(int argc, char *argv[])
return 0;
}
+
+void
+Navigator::publish_mission_result()
+{
+ /* lazily publish the mission result only once available */
+ if (_mission_result_pub > 0) {
+ /* publish mission result */
+ orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+ } else {
+ /* advertise and publish */
+ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+ }
+ /* reset reached bool */
+ _mission_result.reached = false;
+ _mission_result.finished = false;
+}
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index f43215665..3c6754c55 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
if (_first_run) {
/* first run */
_first_run = false;
+ /* Reset stay in failsafe flag */
+ _navigator->get_mission_result()->stay_in_failsafe = true;
+ _navigator->publish_mission_result();
on_activation();
} else {
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);
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index beb797e62..65ddfb4ad 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -57,6 +57,7 @@ struct mission_result_s
unsigned seq_current; /**< Sequence of the current mission item */
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
+ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
};
/**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index ec2131abd..1c78f5330 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -97,6 +97,7 @@ struct position_setpoint_triplet_s
struct position_setpoint_s next;
unsigned nav_state; /**< report the navigation state */
+ bool geofence_violated; /**< true if the geofence is violated */
};
/**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b683bf98a..301503b82 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -103,6 +103,8 @@ typedef enum {
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
+ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */
+ NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
@@ -200,7 +202,11 @@ struct vehicle_status_s {
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_input_blocked; /**< set if RC input should be ignored */
- bool data_link_lost; /**< datalink to GCS lost */
+ bool data_link_lost; /**< datalink to GCS lost */
+ uint8_t data_link_lost_counter; /**< counts unique data link lost events */
+
+ bool engine_failure; /** Set to true if an engine failure is detected */
+ bool gps_failure; /** Set to true if a gps failure is detected */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 23:06:40 +0000