From aee1e706421d80e6fc64a336503110006c3e7d32 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 18 Feb 2020 17:35:12 +0100 Subject: [PATCH] check avoidance status in commander and set sys status remove mavlink log --- msg/subsystem_info.msg | 1 + src/modules/commander/Commander.cpp | 62 +++++++++++++++++------- src/modules/commander/Commander.hpp | 13 +++-- src/modules/commander/commander_params.c | 13 ----- 4 files changed, 55 insertions(+), 34 deletions(-) diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 389d5d6183..aecdd98830 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -28,6 +28,7 @@ uint64 SUBSYSTEM_TYPE_LOGGING = 16777216 uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432 uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864 uint64 SUBSYSTEM_TYPE_SATCOM = 134217728 +uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912 bool present bool enabled diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 476f88c482..e1e7c8e729 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1976,6 +1976,8 @@ Commander::run() // data link checks which update the status data_link_check(); + avoidance_check(); + // engine failure detection // TODO: move out of commander if (_actuator_controls_sub.updated()) { @@ -3614,7 +3616,6 @@ void Commander::data_link_check() _datalink_last_status_avoidance_system = telemetry.remote_system_status; if (_avoidance_system_lost) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system regained"); _status_changed = true; _avoidance_system_lost = false; status_flags.avoidance_system_valid = true; @@ -3654,42 +3655,29 @@ void Commander::data_link_check() // AVOIDANCE SYSTEM state check (only if it is enabled) if (status_flags.avoidance_system_required && !_onboard_controller_lost) { - //if avoidance never started - if (_datalink_last_heartbeat_avoidance_system == 0 - && hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) { - if (!_print_avoidance_msg_once) { - mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available"); - _print_avoidance_msg_once = true; - - } - } - //if heartbeats stop if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { _avoidance_system_lost = true; - mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost"); status_flags.avoidance_system_valid = false; - _print_avoidance_msg_once = false; } //if status changed if (_avoidance_system_status_change) { if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system starting"); + status_flags.avoidance_system_valid = false; } if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system connected"); status_flags.avoidance_system_valid = true; } if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) { - mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out"); + status_flags.avoidance_system_valid = false; + _status_changed = true; } if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) { - mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected"); status_flags.avoidance_system_valid = false; _status_changed = true; } @@ -3712,6 +3700,46 @@ void Commander::data_link_check() } } +void Commander::avoidance_check() +{ + + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_sub_distance_sensor[i].updated()) { + distance_sensor_s distance_sensor {}; + _sub_distance_sensor[i].copy(&distance_sensor); + + if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + _valid_distance_sensor_time_us = distance_sensor.timestamp; + } + } + } + + const bool cp_enabled = _param_cp_dist.get() > 0.f; + + const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms; + const bool cp_healthy = status_flags.avoidance_system_valid || distance_sensor_valid; + + const bool sensor_oa_present = cp_healthy || status_flags.avoidance_system_required || cp_enabled; + + const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION + || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER + || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL + || _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD + || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF + || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; + const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL; + + const bool sensor_oa_enabled = ((auto_mode && status_flags.avoidance_system_required) || (pos_ctl_mode + && cp_enabled)) ? true : false; + const bool sensor_oa_healthy = ((auto_mode && status_flags.avoidance_system_valid) || (pos_ctl_mode + && cp_healthy)) ? true : false; + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled, + sensor_oa_healthy, status); + +} + void Commander::battery_status_check() { bool battery_sub_updated = false; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 09ec70c898..1fc2fa08e6 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -132,6 +133,8 @@ private: */ void data_link_check(); + void avoidance_check(); + void esc_status_check(const esc_status_s &esc_status); void estimator_check(); @@ -196,7 +199,6 @@ private: (ParamFloat) _param_com_disarm_preflight, (ParamBool) _param_com_obs_avoid, - (ParamInt) _param_com_oa_boot_t, (ParamInt) _param_com_flt_profile, @@ -251,7 +253,9 @@ private: // Mavlink (ParamInt) _param_mav_comp_id, (ParamInt) _param_mav_sys_id, - (ParamInt) _param_mav_type + (ParamInt) _param_mav_type, + + (ParamFloat) _param_cp_dist ) enum class PrearmedMode { @@ -282,6 +286,8 @@ private: PreFlightCheck::arm_requirements_t _arm_requirements{}; + hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */ + hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */ hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */ hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */ @@ -325,8 +331,6 @@ private: Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_killed{false}; - bool _print_avoidance_msg_once{false}; - hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent float _eph_threshold_adj{INFINITY}; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition @@ -388,6 +392,7 @@ private: #endif uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */ uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)}; uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index c57ba293a1..44785b7772 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -844,19 +844,6 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); */ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); -/** - * Set avoidance system bootup timeout. - * - * The avoidance system running on the companion computer is expected to boot - * within this time and start providing trajectory points. - * If no avoidance system is detected a MAVLink warning message is sent. - * @group Commander - * @unit s - * @min 0 - * @max 200 - */ -PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100); - /** * User Flight Profile *