From 9966a3d2f8ba88239054d3224d52726766cea724 Mon Sep 17 00:00:00 2001
From: Claudio Micheli <claudio@auterion.com>
Date: Thu, 14 Mar 2019 17:48:07 +0100
Subject: [PATCH] Commander: changed logic for checking OA at boot time.
With this commit the use cases will be:
Success case:
- booting, no messages about OA, pre-arm check would fail if you try to arm and OA is not yet running
Fail case:
- if OA takes longer than timeout time defined in COM_ONB_BOOT_T, then an error message is triggered.
Signed-off-by: Claudio Micheli <claudio@auterion.com>
---
src/modules/commander/Commander.cpp | 28 +++++++++++-------------
src/modules/commander/commander_params.c | 11 ++++------
2 files changed, 17 insertions(+), 22 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index a22e8a21c9..4c9d318297 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -123,7 +123,6 @@ static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sens
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms;
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms;
-static constexpr uint8_t AVOIDANCE_MAX_TRIALS = 4; /* Maximum number of trials for avoidance system to start */
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
static orb_advert_t power_button_state_pub = nullptr;
@@ -174,8 +173,6 @@ static float _eph_threshold_adj =
INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
static bool _skip_pos_accuracy_check = false;
-static int avoidance_waiting_count = 0;
-
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -584,7 +581,6 @@ Commander::Commander() :
status_flags.condition_power_input_valid = true;
status_flags.rc_calibration_valid = true;
- status_flags.avoidance_system_required = _obs_avoid.get();
status_flags.avoidance_system_valid = false;
@@ -1218,6 +1214,8 @@ Commander::run()
/* failsafe response to loss of navigation accuracy */
param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL");
+ status_flags.avoidance_system_required = _obs_avoid.get();
+
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -1486,6 +1484,9 @@ Commander::run()
param_init_forced = false;
}
+ /* Update OA parameter */
+ status_flags.avoidance_system_required = _obs_avoid.get();
+
/* handle power button state */
orb_check(power_button_state_sub, &updated);
@@ -3833,6 +3834,7 @@ bool Commander::preflight_check(bool report)
void Commander::data_link_check(bool &status_changed)
{
bool updated = false;
+ static bool print_msg_once = false;
orb_check(_telemetry_status_sub, &updated);
@@ -3965,15 +3967,12 @@ void Commander::data_link_check(bool &status_changed)
//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0
- && hrt_elapsed_time(&_avoidance_system_not_started) > _onboard_boot_timeout.get() * 1_s
- && avoidance_waiting_count < AVOIDANCE_MAX_TRIALS) {
- _avoidance_system_not_started = hrt_absolute_time();
- mavlink_log_info(&mavlink_log_pub, "Waiting for avoidance system to start");
- avoidance_waiting_count++;
-
- } else if (avoidance_waiting_count == AVOIDANCE_MAX_TRIALS) {
- mavlink_log_critical(&mavlink_log_pub, "Avoidance not responding. Try reboot vehicle.");
- avoidance_waiting_count++;
+ && hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _onboard_boot_timeout.get() * 1_s) {
+ if (!print_msg_once) {
+ mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available!");
+ print_msg_once = true;
+
+ }
}
//if heartbeats stop
@@ -3991,9 +3990,8 @@ void Commander::data_link_check(bool &status_changed)
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
- mavlink_log_info(&mavlink_log_pub, "Avoidance system healthy");
+ mavlink_log_info(&mavlink_log_pub, "Avoidance system connected");
status_flags.avoidance_system_valid = true;
- avoidance_waiting_count = 0;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 336bceafdc..b5d442637e 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -799,10 +799,8 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
/**
* Flag to enable obstacle avoidance
- * Temporary Parameter to enable interface testing
*
* @boolean
- * @reboot_required true
* @group Mission
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
@@ -811,12 +809,11 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
* Set onboard controller bootup timeout
*
* This parameter defines the bootup timeout.
- * After the timeout a mavlink message to warn the user that the system
- * is still booting up is triggered.
- *
+ * After the timeout, a mavlink message that tells the user that the avoidance system
+ * is not available is sent.
* @group Commander
* @unit s
* @min 0
- * @max 120
+ * @max 200
*/
-PARAM_DEFINE_INT32(COM_ONB_BOOT_T, 15);
\ No newline at end of file
+PARAM_DEFINE_INT32(COM_ONB_BOOT_T, 100);
\ No newline at end of file
--
GitLab