aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander')
-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
4 files changed, 87 insertions, 15 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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 02:52:35 +0000